diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index e298dccefd827..01ccc632b0133 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -19,9 +19,9 @@ common/global_parameter_loader/** ryohsuke.mitsudome@tier4.jp common/glog_component/** takamasa.horibe@tier4.jp common/goal_distance_calculator/** taiki.tanaka@tier4.jp common/grid_map_utils/** maxime.clement@tier4.jp -common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/interpolation/** fumiya.watanabe@tier4.jp takayuki.murooka@tier4.jp common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.saito@tier4.jp -common/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 yutaka.shimizu@tier4.jp +common/motion_utils/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp common/object_recognition_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp yusuke.muramatsu@tier4.jp common/osqp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp common/path_distance_calculator/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp @@ -34,8 +34,9 @@ common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato. common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_automatic_goal_rviz_plugin/** dawid.moszynski@robotec.ai shumpei.wakabayashi@tier4.jp -common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_autoware_utils/** mamoru.sobue@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tier4_calibration_rviz_plugin/** tomoya.kimura@tier4.jp +common/tier4_camera_view_rviz_plugin/** makoto.ybauta@tier4.jp uken.ryu@tier4.jp common/tier4_control_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_datetime_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp @@ -43,7 +44,7 @@ common/tier4_debug_tools/** ryohsuke.mitsudome@tier4.jp common/tier4_localization_rviz_plugin/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp takamasa.horibe@tier4.jp common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp -common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp yutaka.shimizu@tier4.jp +common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp common/tier4_screen_capture_rviz_plugin/** taiki.tanaka@tier4.jp common/tier4_simulated_clock_rviz_plugin/** maxime.clement@tier4.jp common/tier4_state_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp @@ -54,9 +55,8 @@ common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_recognition_marker_publisher/** shumpei.wakabayashi@tier4.jp takeshi.miura@tier4.jp tomoya.kimura@tier4.jp common/traffic_light_utils/** mingyu.li@tier4.jp shunsuke.miura@tier4.jp -common/trtexec_vendor/** daisuke.nishimatsu@tier4.jp yusuke.muramatsu@tier4.jp common/tvm_utility/** ambroise.vincent@arm.com xinyu.wang@tier4.jp -control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +control/autonomous_emergency_braking/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp control/control_performance_analysis/** berkay@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp control/control_validator/** kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp control/external_cmd_selector/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp @@ -81,20 +81,22 @@ launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.j launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp launch/tier4_map_launch/** koji.minoda@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp launch/tier4_perception_launch/** shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp -launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +launch/tier4_planning_launch/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp launch/tier4_sensing_launch/** yukihiro.saito@tier4.jp launch/tier4_simulator_launch/** keisuke.shima@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -launch/tier4_system_launch/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +launch/tier4_system_launch/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp localization/ekf_localizer/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** koji.minoda@tier4.jp yamato.ando@tier4.jp localization/gyro_odometer/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/landmark_based_localizer/ar_tag_based_localizer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp -localization/landmark_based_localizer/landmark_parser/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/localization_error_monitor/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/localization_util/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/ndt_scan_matcher/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose2twist/** masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/pose_initializer/** isamu.takagi@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** 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/stop_filter/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp localization/tree_structured_parzen_estimator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp shintaro.sakoda@tier4.jp yamato.ando@tier4.jp localization/twist2accel/** koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp @@ -113,20 +115,20 @@ perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihir perception/compare_map_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp yukihiro.saito@tier4.jp perception/crosswalk_traffic_light_estimator/** satoshi.ota@tier4.jp shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/detected_object_feature_remover/** tomoya.kimura@tier4.jp -perception/detected_object_validation/** shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp +perception/detected_object_validation/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/detection_by_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/elevation_map_loader/** kosuke.takeuchi@tier4.jp taichi.higashide@tier4.jp perception/euclidean_cluster/** yukihiro.saito@tier4.jp perception/front_vehicle_velocity_estimator/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp perception/ground_segmentation/** abrahammonrroy@yahoo.com dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp perception/heatmap_visualizer/** kotaro.uetake@tier4.jp -perception/image_projection_based_fusion/** yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp +perception/image_projection_based_fusion/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yoshi.ri@tier4.jp yukihiro.saito@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_apollo_instance_segmentation/** yukihiro.saito@tier4.jp perception/lidar_apollo_segmentation_tvm/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_apollo_segmentation_tvm_nodes/** ambroise.vincent@arm.com xinyu.wang@tier4.jp perception/lidar_centerpoint/** kenzo.lobos@tier4.jp yusuke.muramatsu@tier4.jp perception/lidar_centerpoint_tvm/** carl.liu@autocore.ai xinyu.wang@tier4.jp -perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp yutaka.shimizu@tier4.jp +perception/map_based_prediction/** kotaro.uetake@tier4.jp kyoichi.sugahara@tier4.jp shunsuke.miura@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yoshi.ri@tier4.jp perception/multi_object_tracker/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_merger/** yoshi.ri@tier4.jp yukihiro.saito@tier4.jp perception/object_range_splitter/** yukihiro.saito@tier4.jp @@ -152,7 +154,8 @@ perception/traffic_light_multi_camera_fusion/** shunsuke.miura@tier4.jp tao.zhon perception/traffic_light_occlusion_predictor/** shunsuke.miura@tier4.jp tao.zhong@tier4.jp perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp -planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp +planning/behavior_path_planner_common/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_velocity_blind_spot_module/** mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_crosswalk_module/** kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -161,31 +164,33 @@ planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_occlusion_spot_module/** shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_out_of_lane_module/** maxime.clement@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp -planning/behavior_velocity_planner_common/** isamu.takagi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp makoto.kurihara@tier4.jp mamoru.sobue@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_planner_common/** fumiya.watanabe@tier4.jp isamu.takagi@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_run_out_module/** makoto.kurihara@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_speed_bump_module/** mdogru@leodrive.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp -planning/behavior_velocity_stop_line_module/** shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp yutaka.shimizu@tier4.jp +planning/behavior_velocity_stop_line_module/** fumiya.watanabe@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp zhe.shen@tier4.jp planning/behavior_velocity_template_module/** daniel.sanchez@tier4.jp planning/behavior_velocity_traffic_light_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_virtual_traffic_light_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_walkway_module/** satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp -planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/costmap_generator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/external_velocity_limit_selector/** satoshi.ota@tier4.jp shinnosuke.hirakawa@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp -planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp -planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp yutaka.shimizu@tier4.jp +planning/freespace_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/freespace_planning_algorithms/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/mission_planner/** isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp ryohsuke.mitsudome@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp +planning/motion_velocity_smoother/** fumiya.watanabe@tier4.jp makoto.kurihara@tier4.jp takamasa.horibe@tier4.jp +planning/objects_of_interest_marker_interface/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp zulfaqar.azmi@tier4.jp planning/obstacle_avoidance_planner/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp +planning/obstacle_cruise_planner/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp planning/obstacle_stop_planner/** berkay@leodrive.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp planning/obstacle_velocity_limiter/** maxime.clement@tier4.jp planning/path_smoother/** maxime.clement@tier4.jp takayuki.murooka@tier4.jp planning/planning_debug_tools/** taiki.tanaka@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp planning/planning_test_utils/** kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp +planning/planning_topic_converter/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp planning/planning_validator/** kosuke.takeuchi@tier4.jp takamasa.horibe@tier4.jp -planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp yutaka.shimizu@tier4.jp zulfaqar.azmi@tier4.jp -planning/rtc_interface/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp +planning/route_handler/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp mamoru.sobue@tier4.jp takayuki.murooka@tier4.jp zulfaqar.azmi@tier4.jp +planning/rtc_interface/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp taiki.tanaka@tier4.jp planning/rtc_replayer/** fumiya.watanabe@tier4.jp taiki.tanaka@tier4.jp planning/sampling_based_planner/bezier_sampler/** maxime.clement@tier4.jp planning/sampling_based_planner/frenet_planner/** maxime.clement@tier4.jp @@ -216,15 +221,15 @@ system/default_ad_api/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabut system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp -system/dummy_diag_publisher/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp system/duplicated_node_checker/** shumpei.wakabayashi@tier4.jp uken.ryu@tier4.jp system/emergency_handler/** makoto.kurihara@tier4.jp -system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp -system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp +system/mrm_comfortable_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp +system/mrm_emergency_stop_operator/** makoto.kurihara@tier4.jp tomohito.ando@tier4.jp system/system_diagnostic_graph/** isamu.takagi@tier4.jp system/system_error_monitor/** fumihito.ito@tier4.jp -system/system_monitor/** akihiro.sakurai@tier4.jp fumihito.ito@tier4.jp +system/system_monitor/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/topic_state_monitor/** ryohsuke.mitsudome@tier4.jp system/velodyne_monitor/** fumihito.ito@tier4.jp tools/simulator_test/simulator_compatibility_test/** shpark@morai.ai shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp diff --git a/.github/workflows/spell-check-all.yaml b/.github/workflows/spell-check-all.yaml index 4feea0dbadcf7..057bd78da2a82 100644 --- a/.github/workflows/spell-check-all.yaml +++ b/.github/workflows/spell-check-all.yaml @@ -2,6 +2,8 @@ name: spell-check-all on: workflow_dispatch: + schedule: + - cron: 0 0 * * * jobs: spell-check-all: diff --git a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml index 5b8c019de5a20..46738a33ae0bd 100644 --- a/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml +++ b/common/goal_distance_calculator/config/goal_distance_calculator.param.yaml @@ -1,3 +1,4 @@ /**: ros__parameters: update_rate: 10.0 + oneshot: false diff --git a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml index 7a796049384bc..7ba2eb45c9233 100644 --- a/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml +++ b/common/goal_distance_calculator/launch/goal_distance_calculator.launch.xml @@ -1,8 +1,6 @@ - - diff --git a/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json new file mode 100644 index 0000000000000..bc8d7d808619f --- /dev/null +++ b/common/goal_distance_calculator/schema/goal_distance_calculator.schema.json @@ -0,0 +1,36 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Goal Distance Calculator Node", + "type": "object", + "definitions": { + "goal_distance_calculator": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": "10.0", + "exclusiveMinimum": 0, + "description": "Timer callback period. [Hz]" + }, + "oneshot": { + "type": "boolean", + "default": "false", + "description": "Publish deviations just once or repeatedly." + } + }, + "required": ["update_rate", "oneshot"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/goal_distance_calculator" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp index 6b147ed66d690..c6062d2a156ee 100644 --- a/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp +++ b/common/goal_distance_calculator/src/goal_distance_calculator_node.cpp @@ -40,8 +40,8 @@ GoalDistanceCalculatorNode::GoalDistanceCalculatorNode(const rclcpp::NodeOptions durable_qos.transient_local(); // Node Parameter - node_param_.update_rate = declare_parameter("update_rate", 10.0); - node_param_.oneshot = declare_parameter("oneshot", true); + node_param_.update_rate = declare_parameter("update_rate"); + node_param_.oneshot = declare_parameter("oneshot"); // Core goal_distance_calculator_ = std::make_unique(); diff --git a/common/interpolation/test/src/test_spline_interpolation.cpp b/common/interpolation/test/src/test_spline_interpolation.cpp index c95fc902ade44..94baf50992cf5 100644 --- a/common/interpolation/test/src/test_spline_interpolation.cpp +++ b/common/interpolation/test/src/test_spline_interpolation.cpp @@ -222,16 +222,48 @@ TEST(spline_interpolation, splineByAkima) TEST(spline_interpolation, SplineInterpolation) { - // curve: query_keys is random - const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; - const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; - const std::vector query_keys{0.0, 8.0, 18.0}; - const std::vector ans{-0.075611, 0.997242, 1.573258}; + { + // curve: query_keys is random + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 18.0}; + const std::vector ans{-0.075611, 0.997242, 1.573258}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedDiffValues + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; + const std::vector ans{0.671301, 0.0509853, 0.209426, -0.253628}; - SplineInterpolation s(base_keys, base_values); - const std::vector query_values = s.getSplineInterpolatedValues(query_keys); + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedDiffValues(query_keys); - for (size_t i = 0; i < query_values.size(); ++i) { - EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } + } + + { + // getSplineInterpolatedQuadDiffValues + const std::vector base_keys{-1.5, 1.0, 5.0, 10.0, 15.0, 20.0}; + const std::vector base_values{-1.2, 0.5, 1.0, 1.2, 2.0, 1.0}; + const std::vector query_keys{0.0, 8.0, 12.0, 18.0}; + const std::vector ans{-0.156582, 0.0440771, -0.0116873, -0.0495025}; + + SplineInterpolation s(base_keys, base_values); + const std::vector query_values = s.getSplineInterpolatedQuadDiffValues(query_keys); + + for (size_t i = 0; i < query_values.size(); ++i) { + EXPECT_NEAR(query_values.at(i), ans.at(i), epsilon); + } } } diff --git a/common/motion_utils/include/motion_utils/distance/distance.hpp b/common/motion_utils/include/motion_utils/distance/distance.hpp index 2e25f5f0e53d7..0a0d00fc9f260 100644 --- a/common/motion_utils/include/motion_utils/distance/distance.hpp +++ b/common/motion_utils/include/motion_utils/distance/distance.hpp @@ -15,17 +15,16 @@ #ifndef MOTION_UTILS__DISTANCE__DISTANCE_HPP_ #define MOTION_UTILS__DISTANCE__DISTANCE_HPP_ -#include - #include #include #include +#include #include #include namespace motion_utils { -boost::optional calcDecelDistWithJerkAndAccConstraints( +std::optional calcDecelDistWithJerkAndAccConstraints( const double current_vel, const double target_vel, const double current_acc, const double acc_min, const double jerk_acc, const double jerk_dec); diff --git a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp index 41a9e9dd41a40..c23478605c41a 100644 --- a/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/interpolation.hpp @@ -21,10 +21,9 @@ #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include - #include #include +#include #include #include diff --git a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp index 74862a6229a46..e088285147735 100644 --- a/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/path_with_lane_id.hpp @@ -18,12 +18,11 @@ #include "autoware_auto_planning_msgs/msg/path_with_lane_id.hpp" #include -#include - +#include #include namespace motion_utils { -boost::optional> getPathIndexRangeWithLaneId( +std::optional> getPathIndexRangeWithLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id); size_t findNearestIndexFromLaneId( diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index f4f9f45935347..cb815f7dd0a0c 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -26,10 +26,9 @@ #include #include -#include - #include #include +#include #include #include #include @@ -93,10 +92,10 @@ void validateNonSharpAngle( * @return (forward / backward) driving (true / false) */ template -boost::optional isDrivingForward(const T & points) +std::optional isDrivingForward(const T & points) { if (points.size() < 2) { - return boost::none; + return std::nullopt; } // check the first point direction @@ -106,13 +105,13 @@ boost::optional isDrivingForward(const T & points) return tier4_autoware_utils::isDrivingForward(first_pose, second_pose); } -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForward>( const std::vector &); @@ -123,10 +122,10 @@ isDrivingForward> * @return (forward / backward) driving (true, false, none "if velocity is zero") */ template -boost::optional isDrivingForwardWithTwist(const T & points_with_twist) +std::optional isDrivingForwardWithTwist(const T & points_with_twist) { if (points_with_twist.empty()) { - return boost::none; + return std::nullopt; } if (points_with_twist.size() == 1) { if (0.0 < tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { @@ -134,20 +133,20 @@ boost::optional isDrivingForwardWithTwist(const T & points_with_twist) } else if (0.0 > tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.front())) { return false; } else { - return boost::none; + return std::nullopt; } } return isDrivingForward(points_with_twist); } -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); -extern template boost::optional +extern template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -209,7 +208,7 @@ removeOverlapPoints -boost::optional searchZeroVelocityIndex( +std::optional searchZeroVelocityIndex( const T & points_with_twist, const size_t src_idx, const size_t dst_idx) { try { @@ -229,7 +228,7 @@ boost::optional searchZeroVelocityIndex( return {}; } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); @@ -242,7 +241,7 @@ searchZeroVelocityIndex -boost::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) +std::optional searchZeroVelocityIndex(const T & points_with_twist, const size_t src_idx) { try { validateNonEmpty(points_with_twist); @@ -254,7 +253,7 @@ boost::optional searchZeroVelocityIndex(const T & points_with_twist, con return searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx); @@ -266,12 +265,12 @@ searchZeroVelocityIndex -boost::optional searchZeroVelocityIndex(const T & points_with_twist) +std::optional searchZeroVelocityIndex(const T & points_with_twist) { return searchZeroVelocityIndex(points_with_twist, 0, points_with_twist.size()); } -extern template boost::optional +extern template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist); @@ -329,7 +328,7 @@ findNearestIndex> * @return index of nearest point (index or none if not found) */ template -boost::optional findNearestIndex( +std::optional findNearestIndex( const T & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) @@ -363,20 +362,24 @@ boost::optional findNearestIndex( min_idx = i; is_nearest_found = true; } - return is_nearest_found ? boost::optional(min_idx) : boost::none; + + if (is_nearest_found) { + return min_idx; + } + return std::nullopt; } -extern template boost::optional +extern template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), @@ -507,7 +510,7 @@ findNearestSegmentIndex -boost::optional findNearestSegmentIndex( +std::optional findNearestSegmentIndex( const T & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) @@ -515,7 +518,7 @@ boost::optional findNearestSegmentIndex( const auto nearest_idx = findNearestIndex(points, pose, max_dist, max_yaw); if (!nearest_idx) { - return boost::none; + return std::nullopt; } if (*nearest_idx == 0) { @@ -534,17 +537,17 @@ boost::optional findNearestSegmentIndex( return *nearest_idx; } -extern template boost::optional +extern template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), @@ -998,7 +1001,7 @@ calcCurvatureAndArcLength -boost::optional calcDistanceToForwardStopPoint( +std::optional calcDistanceToForwardStopPoint( const T & points_with_twist, const size_t src_idx = 0) { try { @@ -1011,13 +1014,13 @@ boost::optional calcDistanceToForwardStopPoint( const auto closest_stop_idx = searchZeroVelocityIndex(points_with_twist, src_idx, points_with_twist.size()); if (!closest_stop_idx) { - return boost::none; + return std::nullopt; } return std::max(0.0, calcSignedArcLength(points_with_twist, src_idx, *closest_stop_idx)); } -extern template boost::optional +extern template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const size_t src_idx = 0); @@ -1032,7 +1035,7 @@ calcDistanceToForwardStopPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const size_t src_idx, const double offset, const bool throw_exception = false) { try { @@ -1087,15 +1090,15 @@ boost::optional calcLongitudinalOffsetPoint( return {}; } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception = false); @@ -1109,7 +1112,7 @@ calcLongitudinalOffsetPoint -boost::optional calcLongitudinalOffsetPoint( +std::optional calcLongitudinalOffsetPoint( const T & points, const geometry_msgs::msg::Point & src_point, const double offset) { try { @@ -1132,15 +1135,15 @@ boost::optional calcLongitudinalOffsetPoint( return calcLongitudinalOffsetPoint(points, src_seg_idx, offset + signed_length_src_offset); } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); @@ -1156,7 +1159,7 @@ calcLongitudinalOffsetPoint -boost::optional calcLongitudinalOffsetPose( +std::optional calcLongitudinalOffsetPose( const T & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false) { @@ -1228,17 +1231,17 @@ boost::optional calcLongitudinalOffsetPose( return {}; } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction = true, const bool throw_exception = false); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, @@ -1255,7 +1258,7 @@ calcLongitudinalOffsetPose -boost::optional calcLongitudinalOffsetPose( +std::optional calcLongitudinalOffsetPose( const T & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true) { @@ -1275,17 +1278,17 @@ boost::optional calcLongitudinalOffsetPose( set_orientation_from_position_direction); } -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction = true); -extern template boost::optional +extern template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, @@ -1301,7 +1304,7 @@ calcLongitudinalOffsetPose -boost::optional insertTargetPoint( +std::optional insertTargetPoint( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { @@ -1339,7 +1342,7 @@ boost::optional insertTargetPoint( geometry_msgs::msg::Pose target_pose; { - const auto p_base = is_driving_forward.get() ? p_back : p_front; + const auto p_base = is_driving_forward.value() ? p_back : p_front; const auto pitch = tier4_autoware_utils::calcElevationAngle(p_target, p_base); const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_target, p_base); @@ -1352,7 +1355,7 @@ boost::optional insertTargetPoint( geometry_msgs::msg::Pose base_pose; { - const auto p_base = is_driving_forward.get() ? p_front : p_back; + const auto p_base = is_driving_forward.value() ? p_front : p_back; const auto pitch = tier4_autoware_utils::calcElevationAngle(p_base, p_target); const auto yaw = tier4_autoware_utils::calcAzimuthAngle(p_base, p_target); @@ -1361,7 +1364,7 @@ boost::optional insertTargetPoint( } if (!overlap_with_front && !overlap_with_back) { - if (is_driving_forward.get()) { + if (is_driving_forward.value()) { tier4_autoware_utils::setPose(base_pose, points.at(seg_idx)); } else { tier4_autoware_utils::setPose(base_pose, points.at(seg_idx + 1)); @@ -1377,17 +1380,17 @@ boost::optional insertTargetPoint( return seg_idx; } -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, @@ -1404,18 +1407,18 @@ insertTargetPoint * @return index of segment id, where point is inserted */ template -boost::optional insertTargetPoint( +std::optional insertTargetPoint( const double insert_point_length, const geometry_msgs::msg::Point & p_target, T & points, const double overlap_threshold = 1e-3) { validateNonEmpty(points); if (insert_point_length < 0.0) { - return boost::none; + return std::nullopt; } // Get Nearest segment index - boost::optional segment_idx = boost::none; + std::optional segment_idx = std::nullopt; for (size_t i = 1; i < points.size(); ++i) { // TODO(Mamoru Sobue): find accumulated sum beforehand const double length = calcSignedArcLength(points, 0, i); @@ -1426,23 +1429,23 @@ boost::optional insertTargetPoint( } if (!segment_idx) { - return boost::none; + return std::nullopt; } return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } -extern template boost::optional +extern template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, @@ -1459,18 +1462,18 @@ insertTargetPoint * @return index of insert point */ template -boost::optional insertTargetPoint( +std::optional insertTargetPoint( const size_t src_segment_idx, const double insert_point_length, T & points, const double overlap_threshold = 1e-3) { validateNonEmpty(points); if (src_segment_idx >= points.size() - 1) { - return boost::none; + return std::nullopt; } // Get Nearest segment index - boost::optional segment_idx = boost::none; + std::optional segment_idx = std::nullopt; if (0.0 <= insert_point_length) { for (size_t i = src_segment_idx + 1; i < points.size(); ++i) { const double length = calcSignedArcLength(points, src_segment_idx, i); @@ -1490,7 +1493,7 @@ boost::optional insertTargetPoint( } if (!segment_idx) { - return boost::none; + return std::nullopt; } // Get Target Point @@ -1505,17 +1508,17 @@ boost::optional insertTargetPoint( return insertTargetPoint(*segment_idx, p_target, points, overlap_threshold); } -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, @@ -1535,7 +1538,7 @@ insertTargetPoint * @return index of insert point */ template -boost::optional insertTargetPoint( +std::optional insertTargetPoint( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, T & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1543,12 +1546,12 @@ boost::optional insertTargetPoint( validateNonEmpty(points); if (insert_point_length < 0.0) { - return boost::none; + return std::nullopt; } const auto nearest_segment_idx = findNearestSegmentIndex(points, src_pose, max_dist, max_yaw); if (!nearest_segment_idx) { - return boost::none; + return std::nullopt; } const double offset_length = @@ -1558,19 +1561,19 @@ boost::optional insertTargetPoint( *nearest_segment_idx, insert_point_length + offset_length, points, overlap_threshold); } -extern template boost::optional +extern template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, @@ -1587,20 +1590,20 @@ insertTargetPoint * @return index of stop point */ template -boost::optional insertStopPoint( +std::optional insertStopPoint( const size_t src_segment_idx, const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { validateNonEmpty(points_with_twist); if (distance_to_stop_point < 0.0 || src_segment_idx >= points_with_twist.size() - 1) { - return boost::none; + return std::nullopt; } const auto stop_idx = insertTargetPoint( src_segment_idx, distance_to_stop_point, points_with_twist, overlap_threshold); if (!stop_idx) { - return boost::none; + return std::nullopt; } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { @@ -1610,17 +1613,17 @@ boost::optional insertStopPoint( return stop_idx; } -extern template boost::optional +extern template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, @@ -1635,13 +1638,13 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +std::optional insertStopPoint( const double distance_to_stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { validateNonEmpty(points_with_twist); if (distance_to_stop_point < 0.0) { - return boost::none; + return std::nullopt; } double accumulated_length = 0; @@ -1656,20 +1659,20 @@ boost::optional insertStopPoint( accumulated_length += length; } - return boost::none; + return std::nullopt; } -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, @@ -1689,7 +1692,7 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +std::optional insertStopPoint( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, T & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3) @@ -1697,14 +1700,14 @@ boost::optional insertStopPoint( validateNonEmpty(points_with_twist); if (distance_to_stop_point < 0.0) { - return boost::none; + return std::nullopt; } const auto stop_idx = insertTargetPoint( src_pose, distance_to_stop_point, points_with_twist, max_dist, max_yaw, overlap_threshold); if (!stop_idx) { - return boost::none; + return std::nullopt; } for (size_t i = *stop_idx; i < points_with_twist.size(); ++i) { @@ -1714,19 +1717,19 @@ boost::optional insertStopPoint( return stop_idx; } -extern template boost::optional +extern template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max(), const double overlap_threshold = 1e-3); -extern template boost::optional +extern template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, @@ -1743,7 +1746,7 @@ insertStopPoint>( * @return index of stop point */ template -boost::optional insertStopPoint( +std::optional insertStopPoint( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, T & points_with_twist, const double overlap_threshold = 1e-3) { @@ -1751,17 +1754,17 @@ boost::optional insertStopPoint( motion_utils::insertTargetPoint(stop_seg_idx, stop_point, points_with_twist, overlap_threshold); if (!insert_idx) { - return boost::none; + return std::nullopt; } - for (size_t i = insert_idx.get(); i < points_with_twist.size(); ++i) { + for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { tier4_autoware_utils::setLongitudinalVelocity(0.0, points_with_twist.at(i)); } return insert_idx; } -extern template boost::optional +extern template std::optional insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, std::vector & points_with_twist, @@ -1775,7 +1778,7 @@ insertStopPoint>( * @param points_with_twist output points of trajectory, path, ... (with velocity) */ template -boost::optional insertDecelPoint( +std::optional insertDecelPoint( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, T & points_with_twist) { @@ -1786,14 +1789,14 @@ boost::optional insertDecelPoint( return {}; } - const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.get()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), points_with_twist); + const auto seg_idx = findNearestSegmentIndex(points_with_twist, decel_point.value()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), points_with_twist); if (!insert_idx) { return {}; } - for (size_t i = insert_idx.get(); i < points_with_twist.size(); ++i) { + for (size_t i = insert_idx.value(); i < points_with_twist.size(); ++i) { const auto & original_velocity = tier4_autoware_utils::getLongitudinalVelocity(points_with_twist.at(i)); tier4_autoware_utils::setLongitudinalVelocity( @@ -1803,7 +1806,7 @@ boost::optional insertDecelPoint( return insert_idx; } -extern template boost::optional +extern template std::optional insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, @@ -2183,7 +2186,7 @@ extern template size_t findFirstNearestSegmentIndexWithSoftConstraints< * longitudinal velocity */ template -boost::optional calcDistanceToForwardStopPoint( +std::optional calcDistanceToForwardStopPoint( const T & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()) @@ -2199,14 +2202,14 @@ boost::optional calcDistanceToForwardStopPoint( motion_utils::findNearestSegmentIndex(points_with_twist, pose, max_dist, max_yaw); if (!nearest_segment_idx) { - return boost::none; + return std::nullopt; } const auto stop_idx = motion_utils::searchZeroVelocityIndex( points_with_twist, *nearest_segment_idx + 1, points_with_twist.size()); if (!stop_idx) { - return boost::none; + return std::nullopt; } const auto closest_stop_dist = @@ -2215,17 +2218,17 @@ boost::optional calcDistanceToForwardStopPoint( return std::max(0.0, closest_stop_dist); } -extern template boost::optional +extern template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), const double max_yaw = std::numeric_limits::max()); -extern template boost::optional +extern template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist = std::numeric_limits::max(), diff --git a/common/motion_utils/src/distance/distance.cpp b/common/motion_utils/src/distance/distance.cpp index 14ba02ab38703..c6de95dd568cf 100644 --- a/common/motion_utils/src/distance/distance.cpp +++ b/common/motion_utils/src/distance/distance.cpp @@ -91,7 +91,7 @@ std::tuple update( * @param (t_during_min_acc) duration of constant deceleration [s] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType1( +std::optional calcDecelDistPlanType1( const double v0, const double vt, const double a0, const double am, const double ja, const double jd, const double t_during_min_acc) { @@ -155,7 +155,7 @@ boost::optional calcDecelDistPlanType1( * @param (jd) minimum jerk [m/sss] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType2( +std::optional calcDecelDistPlanType2( const double v0, const double vt, const double a0, const double ja, const double jd) { constexpr double epsilon = 1e-3; @@ -212,7 +212,7 @@ boost::optional calcDecelDistPlanType2( * @param (ja) maximum jerk [m/sss] * @return moving distance until velocity is reached vt [m] */ -boost::optional calcDecelDistPlanType3( +std::optional calcDecelDistPlanType3( const double v0, const double vt, const double a0, const double ja) { constexpr double epsilon = 1e-3; @@ -234,7 +234,7 @@ boost::optional calcDecelDistPlanType3( } } // namespace -boost::optional calcDecelDistWithJerkAndAccConstraints( +std::optional calcDecelDistWithJerkAndAccConstraints( const double current_vel, const double target_vel, const double current_acc, const double acc_min, const double jerk_acc, const double jerk_dec) { diff --git a/common/motion_utils/src/trajectory/path_with_lane_id.cpp b/common/motion_utils/src/trajectory/path_with_lane_id.cpp index c74db0f61d6fb..c7e85554dbddd 100644 --- a/common/motion_utils/src/trajectory/path_with_lane_id.cpp +++ b/common/motion_utils/src/trajectory/path_with_lane_id.cpp @@ -24,7 +24,7 @@ namespace motion_utils { -boost::optional> getPathIndexRangeWithLaneId( +std::optional> getPathIndexRangeWithLaneId( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id) { size_t start_idx = 0; // NOTE: to prevent from maybe-uninitialized error diff --git a/common/motion_utils/src/trajectory/trajectory.cpp b/common/motion_utils/src/trajectory/trajectory.cpp index 12074e537c106..51c07a75076c8 100644 --- a/common/motion_utils/src/trajectory/trajectory.cpp +++ b/common/motion_utils/src/trajectory/trajectory.cpp @@ -26,24 +26,24 @@ template void validateNonEmpty &); // -template boost::optional +template std::optional isDrivingForward>( const std::vector &); -template boost::optional +template std::optional isDrivingForward>( const std::vector &); -template boost::optional +template std::optional isDrivingForward>( const std::vector &); // -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); -template boost::optional +template std::optional isDrivingForwardWithTwist>( const std::vector &); @@ -61,19 +61,19 @@ removeOverlapPoints +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx, const size_t dst_idx); // -template boost::optional +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist, const size_t src_idx); // -template boost::optional +template std::optional searchZeroVelocityIndex>( const std::vector & points_with_twist); @@ -90,15 +90,15 @@ template size_t findNearestIndex +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); @@ -131,15 +131,15 @@ findNearestSegmentIndex +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); -template boost::optional +template std::optional findNearestSegmentIndex>( const std::vector & points, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); @@ -261,201 +261,201 @@ calcCurvatureAndArcLength & points); // -template boost::optional +template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const size_t src_idx); // -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const size_t src_idx, const double offset, const bool throw_exception); // -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); -template boost::optional +template std::optional calcLongitudinalOffsetPoint>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset); // -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const size_t src_idx, const double offset, const bool set_orientation_from_position_direction, const bool throw_exception); // -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); -template boost::optional +template std::optional calcLongitudinalOffsetPose>( const std::vector & points, const geometry_msgs::msg::Point & src_point, const double offset, const bool set_orientation_from_position_direction); // -template boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t seg_idx, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); // -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const double insert_point_length, const geometry_msgs::msg::Point & p_target, std::vector & points, const double overlap_threshold); // -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const size_t src_segment_idx, const double insert_point_length, std::vector & points, const double overlap_threshold); // -template boost::optional +template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); -template boost::optional +template std::optional insertTargetPoint>( const geometry_msgs::msg::Pose & src_pose, const double insert_point_length, std::vector & points, const double max_dist, const double max_yaw, const double overlap_threshold); // -template boost::optional +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -template boost::optional +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); -template boost::optional +template std::optional insertStopPoint>( const size_t src_segment_idx, const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold = 1e-3); // -template boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const double distance_to_stop_point, std::vector & points_with_twist, const double overlap_threshold); // -template boost::optional +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); -template boost::optional +template std::optional insertStopPoint>( const geometry_msgs::msg::Pose & src_pose, const double distance_to_stop_point, std::vector & points_with_twist, const double max_dist, const double max_yaw, const double overlap_threshold); // -template boost::optional +template std::optional insertStopPoint>( const size_t stop_seg_idx, const geometry_msgs::msg::Point & stop_point, std::vector & points_with_twist, const double overlap_threshold); // -template boost::optional +template std::optional insertDecelPoint>( const geometry_msgs::msg::Point & src_point, const double distance_to_decel_point, const double velocity, @@ -539,7 +539,7 @@ template size_t findFirstNearestSegmentIndexWithSoftConstraints< const geometry_msgs::msg::Pose & pose, const double dist_threshold, const double yaw_threshold); // -template boost::optional +template std::optional calcDistanceToForwardStopPoint>( const std::vector & points_with_twist, const geometry_msgs::msg::Pose & pose, const double max_dist, const double max_yaw); diff --git a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp index 194c49ae3bff8..900a652033f28 100644 --- a/common/motion_utils/src/vehicle/vehicle_state_checker.cpp +++ b/common/motion_utils/src/vehicle/vehicle_state_checker.cpp @@ -124,7 +124,7 @@ bool VehicleArrivalChecker::isVehicleStoppedAtStopPoint(const double stop_durati return false; } - return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.get())) < + return std::abs(motion_utils::calcSignedArcLength(trajectory_ptr_->points, p, idx.value())) < th_arrived_distance_m; } diff --git a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp index 4df1ef0a094f7..a65147a65b12d 100644 --- a/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp +++ b/common/motion_utils/test/src/trajectory/test_path_with_lane_id.cpp @@ -85,7 +85,7 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) } { const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, boost::none); + EXPECT_EQ(res, std::nullopt); } } @@ -93,7 +93,7 @@ TEST(path_with_lane_id, getPathIndexRangeWithLaneId) { PathWithLaneId points; const auto res = getPathIndexRangeWithLaneId(points, 4); - EXPECT_EQ(res, boost::none); + EXPECT_EQ(res, std::nullopt); } } diff --git a/common/motion_utils/test/src/trajectory/test_trajectory.cpp b/common/motion_utils/test/src/trajectory/test_trajectory.cpp index 6237813a8084c..1786586ec39a8 100644 --- a/common/motion_utils/test/src/trajectory/test_trajectory.cpp +++ b/common/motion_utils/test/src/trajectory/test_trajectory.cpp @@ -856,7 +856,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Boundary1 (Edge of the input trajectory) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 0); - EXPECT_NEAR(dist.get(), 50.0, epsilon); + EXPECT_NEAR(dist.value(), 50.0, epsilon); } // Boundary2 (Edge of the input trajectory) @@ -868,13 +868,13 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Boundary3 (On the Stop Point) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 50); - EXPECT_NEAR(dist.get(), 0.0, epsilon); + EXPECT_NEAR(dist.value(), 0.0, epsilon); } // Boundary4 (Right before the stop point) { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 49); - EXPECT_NEAR(dist.get(), 1.0, epsilon); + EXPECT_NEAR(dist.value(), 1.0, epsilon); } // Boundary5 (Right behind the stop point) @@ -886,7 +886,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromIndex) // Random { const auto dist = calcDistanceToForwardStopPoint(traj_input.points, 20); - EXPECT_NEAR(dist.get(), 30.0, epsilon); + EXPECT_NEAR(dist.value(), 30.0, epsilon); } } @@ -914,7 +914,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 50.0, epsilon); + EXPECT_NEAR(dist.value(), 50.0, epsilon); } // Trajectory Edge2 @@ -928,7 +928,7 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(-10.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 60.0, epsilon); + EXPECT_NEAR(dist.value(), 60.0, epsilon); } // Out of Trajectory2 @@ -942,14 +942,14 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(-30.0, 50.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 80.0, epsilon); + EXPECT_NEAR(dist.value(), 80.0, epsilon); } // Boundary Condition1 { const auto pose = createPose(50.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 0.0, epsilon); + EXPECT_NEAR(dist.value(), 0.0, epsilon); } // Boundary Condition2 @@ -963,14 +963,14 @@ TEST(trajectory, calcDistanceToForwardStopPointFromPose) { const auto pose = createPose(49.9, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 0.1, epsilon); + EXPECT_NEAR(dist.value(), 0.1, epsilon); } // Random { const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } } @@ -985,14 +985,14 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { const auto pose = createPose(-4.9, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 54.9, epsilon); + EXPECT_NEAR(dist.value(), 54.9, epsilon); } // Boundary Condition2 { const auto pose = createPose(-5.0, 0.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 55.0, epsilon); + EXPECT_NEAR(dist.value(), 55.0, epsilon); } // Boundary Condition3 @@ -1007,7 +1007,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_DistThreshold) { const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, 0.0); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, 5.0); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } { @@ -1034,14 +1034,14 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(29.9)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.get(), 48.0, epsilon); + EXPECT_NEAR(dist.value(), 48.0, epsilon); } { const auto pose = createPose(x, 0.0, 0.0, 0.0, 0.0, deg2rad(30.0)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(30.0)); - EXPECT_NEAR(dist.get(), 48.0, epsilon); + EXPECT_NEAR(dist.value(), 48.0, epsilon); } { @@ -1058,7 +1058,7 @@ TEST(trajectory, calcDistanceToForwardStopPoint_YawThreshold) const auto pose = createPose(3.0, 2.0, 0.0, 0.0, 0.0, deg2rad(15.0)); const auto dist = calcDistanceToForwardStopPoint(traj_input.points, pose, max_d, deg2rad(20.0)); - EXPECT_NEAR(dist.get(), 47.0, epsilon); + EXPECT_NEAR(dist.value(), 47.0, epsilon); } { @@ -1096,10 +1096,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans += 0.1; } @@ -1114,10 +1114,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, i, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans -= 0.1; } @@ -1127,14 +1127,14 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, 0, total_length + epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found { const auto p_out = calcLongitudinalOffsetPoint(traj.points, 9, -total_length - epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found(Trajectory size is 1) @@ -1142,7 +1142,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromIndex) const auto one_point_traj = generateTestTrajectory(1, 1.0); const auto p_out = calcLongitudinalOffsetPoint(one_point_traj.points, 0.0, 0.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } } @@ -1171,10 +1171,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans += 0.1; } @@ -1191,10 +1191,10 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().z, 0.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().z, 0.0, epsilon); x_ans -= 0.1; } @@ -1205,7 +1205,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) const auto p_src = createPoint(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, total_length + 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found @@ -1213,7 +1213,7 @@ TEST(trajectory, calcLongitudinalOffsetPointFromPoint) const auto p_src = createPoint(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPoint(traj.points, p_src, -total_length - 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // Out of range(Trajectory size is 1) @@ -1250,14 +1250,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans += 0.1; } @@ -1272,14 +1272,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, i, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans -= 0.1; } @@ -1289,14 +1289,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) { const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length + epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found { const auto p_out = calcLongitudinalOffsetPose(traj.points, 9, -total_length - epsilon); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found(Trajectory size is 1) @@ -1304,7 +1304,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex) const auto one_point_traj = generateTestTrajectory(1, 1.0); const auto p_out = calcLongitudinalOffsetPose(one_point_traj.points, 0.0, 0.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } } @@ -1338,14 +1338,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Found pose(backward) @@ -1353,14 +1353,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, -len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1368,14 +1368,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1383,14 +1383,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1427,14 +1427,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Found pose(backward) @@ -1445,14 +1445,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 * ratio)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0 - len * std::cos(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0 - len * std::sin(deg2rad(45.0)), epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1460,14 +1460,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 0, total_length, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } // Boundary condition @@ -1475,14 +1475,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromIndex_quatSphericalInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, 1, 0.0, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1511,14 +1511,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) for (double len = 0.0; len < d_back + epsilon; len += 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::min(len, d_back)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans += 0.1; } @@ -1535,14 +1535,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) for (double len = 0.0; d_front - epsilon < len; len -= 0.1) { const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, std::max(len, d_front)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, x_ans, epsilon); - EXPECT_NEAR(p_out.get().position.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, 1.0, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, x_ans, epsilon); + EXPECT_NEAR(p_out.value().position.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, 1.0, epsilon); x_ans -= 0.1; } @@ -1553,7 +1553,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) const auto p_src = createPoint(0.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length + 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // No found @@ -1561,7 +1561,7 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint) const auto p_src = createPoint(9.0, 0.0, 0.0); const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, -total_length - 1.0); - EXPECT_EQ(p_out, boost::none); + EXPECT_EQ(p_out, std::nullopt); } // Out of range(Trajectory size is 1) @@ -1612,16 +1612,16 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, len); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0)); - EXPECT_NE(p_out, boost::none); + EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( - p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); EXPECT_NEAR( - p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1635,14 +1635,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatInterpolation) const auto p_out = calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1689,16 +1689,16 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(45.0 - 45.0 * ratio)); - EXPECT_NE(p_out, boost::none); + EXPECT_NE(p_out, std::nullopt); EXPECT_NEAR( - p_out.get().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); + p_out.value().position.x, p_src.x + len * std::cos(deg2rad(45.0)) - deviation, epsilon); EXPECT_NEAR( - p_out.get().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + p_out.value().position.y, p_src.y + len * std::sin(deg2rad(45.0)) + deviation, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1713,14 +1713,14 @@ TEST(trajectory, calcLongitudinalOffsetPoseFromPoint_quatSphericalInterpolation) calcLongitudinalOffsetPose(traj.points, p_src, total_length - src_offset, false); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); - EXPECT_NE(p_out, boost::none); - EXPECT_NEAR(p_out.get().position.x, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.y, 1.0, epsilon); - EXPECT_NEAR(p_out.get().position.z, 0.0, epsilon); - EXPECT_NEAR(p_out.get().orientation.x, ans_quat.x, epsilon); - EXPECT_NEAR(p_out.get().orientation.y, ans_quat.y, epsilon); - EXPECT_NEAR(p_out.get().orientation.z, ans_quat.z, epsilon); - EXPECT_NEAR(p_out.get().orientation.w, ans_quat.w, epsilon); + EXPECT_NE(p_out, std::nullopt); + EXPECT_NEAR(p_out.value().position.x, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.y, 1.0, epsilon); + EXPECT_NEAR(p_out.value().position.z, 0.0, epsilon); + EXPECT_NEAR(p_out.value().orientation.x, ans_quat.x, epsilon); + EXPECT_NEAR(p_out.value().orientation.y, ans_quat.y, epsilon); + EXPECT_NEAR(p_out.value().orientation.z, ans_quat.z, epsilon); + EXPECT_NEAR(p_out.value().orientation.w, ans_quat.w, epsilon); } } @@ -1745,8 +1745,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1754,7 +1754,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1783,8 +1783,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1792,7 +1792,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1821,8 +1821,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1830,7 +1830,7 @@ TEST(trajectory, insertTargetPoint) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -1859,8 +1859,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1876,8 +1876,8 @@ TEST(trajectory, insertTargetPoint) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1895,7 +1895,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -1908,7 +1908,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -1921,7 +1921,7 @@ TEST(trajectory, insertTargetPoint) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid base index @@ -1932,7 +1932,7 @@ TEST(trajectory, insertTargetPoint) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(segment_idx, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -1969,8 +1969,8 @@ TEST(trajectory, insertTargetPoint_Reverse) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -1979,7 +1979,7 @@ TEST(trajectory, insertTargetPoint_Reverse) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(180)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2024,8 +2024,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2034,7 +2034,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2064,8 +2064,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2083,8 +2083,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold) const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2115,8 +2115,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2124,7 +2124,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2153,8 +2153,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1.1e-3, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2162,7 +2162,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2191,8 +2191,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(9.0, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2200,7 +2200,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2229,8 +2229,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2238,7 +2238,7 @@ TEST(trajectory, insertTargetPoint_Length) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2267,8 +2267,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start + 1e-4, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2284,8 +2284,8 @@ TEST(trajectory, insertTargetPoint_Length) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(x_start - 1e-4, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2300,7 +2300,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(-1.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(-1.0, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind the end point) @@ -2310,7 +2310,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertTargetPoint(10.0, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -2322,7 +2322,7 @@ TEST(trajectory, insertTargetPoint_Length) const auto insert_idx = insertTargetPoint(4.0, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -2355,8 +2355,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2364,7 +2364,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2393,8 +2393,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1.1e-3, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2402,7 +2402,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2431,8 +2431,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, 9.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2440,7 +2440,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2469,8 +2469,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start + 1e-4, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2486,8 +2486,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(0, x_start - 1e-4, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2501,7 +2501,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const auto insert_idx = insertTargetPoint(0, -1.0, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind the end point) @@ -2510,7 +2510,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point) const auto insert_idx = insertTargetPoint(0, 10.0, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -2541,8 +2541,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2550,7 +2550,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2581,8 +2581,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2590,7 +2590,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2621,8 +2621,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2630,7 +2630,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2661,8 +2661,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2670,7 +2670,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2701,8 +2701,8 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2710,7 +2710,7 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2734,19 +2734,19 @@ TEST(trajectory, insertTargetPoint_Length_Without_Target_Point_Non_Zero_Start_Id // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(10, 1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(8, 1.0001, traj_out.points), std::nullopt); } } @@ -2771,8 +2771,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2780,7 +2780,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2811,8 +2811,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2820,7 +2820,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2851,8 +2851,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2860,7 +2860,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2891,8 +2891,8 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2900,7 +2900,7 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -2924,16 +2924,16 @@ TEST(trajectory, insertTargetPoint_Negative_Length_Without_Target_Point_Non_Zero // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(0, -1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(9, -10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(9, -9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(1, -1.0001, traj_out.points), std::nullopt); } } @@ -2959,8 +2959,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -2968,7 +2968,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3002,8 +3002,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3011,7 +3011,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3046,8 +3046,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3069,8 +3069,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3078,7 +3078,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3113,8 +3113,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3122,7 +3122,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3151,8 +3151,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 0.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 0.5, traj_out.points), std::nullopt); } // Insert from the point behind the trajectory (Invalid) @@ -3162,9 +3162,9 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points), std::nullopt); } // Insert from the point in front of the trajectory (Boundary Condition) @@ -3179,8 +3179,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3188,7 +3188,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3221,8 +3221,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertTargetPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { @@ -3230,7 +3230,7 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3258,8 +3258,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, -10.0, traj_out.points), std::nullopt); } // No Insert (Too Far from the source point) @@ -3269,8 +3269,8 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), boost::none); - EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), boost::none); + EXPECT_EQ(insertTargetPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertTargetPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); } // No Insert (Too large yaw deviation) @@ -3283,9 +3283,9 @@ TEST(trajectory, insertTargetPoint_Length_from_a_pose) createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); const double max_dist = std::numeric_limits::max(); EXPECT_EQ( - insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + insertTargetPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); EXPECT_EQ( - insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + insertTargetPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); } } @@ -3310,13 +3310,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3324,7 +3324,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3355,13 +3355,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3369,7 +3369,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3400,13 +3400,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3414,7 +3414,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3445,13 +3445,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3459,7 +3459,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3490,13 +3490,13 @@ TEST(trajectory, insertStopPoint_from_a_source_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(start_idx, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3504,7 +3504,7 @@ TEST(trajectory, insertStopPoint_from_a_source_index) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3528,19 +3528,19 @@ TEST(trajectory, insertStopPoint_from_a_source_index) // No Insert (Index Out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(9, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(9, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10, 1.0, traj_out.points), std::nullopt); } // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(0, 10.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(0, 9.0001, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(5, 5.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(8, 1.0001, traj_out.points), std::nullopt); } } @@ -3564,13 +3564,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start + 2.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3578,7 +3578,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3608,13 +3608,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(8.0 + x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3622,7 +3622,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3651,13 +3651,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(9.0, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3665,7 +3665,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3695,13 +3695,13 @@ TEST(trajectory, insertStopPoint_from_front_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3709,7 +3709,7 @@ TEST(trajectory, insertStopPoint_from_front_point) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3733,11 +3733,11 @@ TEST(trajectory, insertStopPoint_from_front_point) // No Insert (Length out of range) { auto traj_out = traj; - EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(9.0 + 1e-2, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(10.0, traj_out.points), std::nullopt); EXPECT_EQ( - insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), boost::none); + insertStopPoint(-std::numeric_limits::epsilon(), traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(-3.0, traj_out.points), std::nullopt); } } @@ -3763,13 +3763,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3777,7 +3777,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3811,13 +3811,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3825,7 +3825,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3860,13 +3860,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3888,13 +3888,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3902,7 +3902,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3937,13 +3937,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -3951,7 +3951,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -3980,8 +3980,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 0.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 0.5, traj_out.points), std::nullopt); } // Insert from the point behind the trajectory (Invalid) @@ -3991,9 +3991,9 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 0.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points), std::nullopt); } // Insert from the point in front of the trajectory (Boundary Condition) @@ -4008,13 +4008,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -4022,7 +4022,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4055,13 +4055,13 @@ TEST(trajectory, insertStopPoint_from_a_pose) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(src_pose, x_start, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 10.0, epsilon); } else { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); @@ -4069,7 +4069,7 @@ TEST(trajectory, insertStopPoint_from_a_pose) } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4097,8 +4097,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, -1.0, traj_out.points), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, -10.0, traj_out.points), std::nullopt); } // No Insert (Too Far from the source point) @@ -4108,8 +4108,8 @@ TEST(trajectory, insertStopPoint_from_a_pose) const double src_pose_y = 3.0; const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, 0.0); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, 1.0), std::nullopt); + EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, 1.0), std::nullopt); } // No Insert (Too large yaw deviation) @@ -4121,8 +4121,9 @@ TEST(trajectory, insertStopPoint_from_a_pose) const geometry_msgs::msg::Pose src_pose = createPose(src_pose_x, src_pose_y, 0.0, 0.0, 0.0, src_yaw); const double max_dist = std::numeric_limits::max(); - EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), boost::none); - EXPECT_EQ(insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), boost::none); + EXPECT_EQ(insertStopPoint(src_pose, 1.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); + EXPECT_EQ( + insertStopPoint(src_pose, 10.0, traj_out.points, max_dist, deg2rad(45)), std::nullopt); } } @@ -4147,23 +4148,23 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4192,22 +4193,22 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4236,22 +4237,22 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } { - const auto p_insert = getPose(traj_out.points.at(insert_idx.get())); + const auto p_insert = getPose(traj_out.points.at(insert_idx.value())); const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0)); EXPECT_EQ(p_insert.position.x, p_target.x); EXPECT_EQ(p_insert.position.y, p_target.y); @@ -4280,17 +4281,17 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } } @@ -4303,17 +4304,17 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); } - for (size_t i = 0; i < insert_idx.get(); ++i) { + for (size_t i = 0; i < insert_idx.value(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 3.0, epsilon); } - for (size_t i = insert_idx.get(); i < traj_out.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < traj_out.points.size(); ++i) { EXPECT_NEAR(traj_out.points.at(i).longitudinal_velocity_mps, 0.0, epsilon); } } @@ -4328,7 +4329,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Behind of end point) @@ -4341,7 +4342,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid target point(Huge lateral offset) @@ -4354,7 +4355,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto insert_idx = insertStopPoint(base_idx, p_target, traj_out.points); EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n"); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Invalid base index @@ -4365,7 +4366,7 @@ TEST(trajectory, insertStopPoint_with_pose_and_segment_index) const auto p_target = createPoint(10.0, 0.0, 0.0); const auto insert_idx = insertStopPoint(segment_idx, p_target, traj_out.points); - EXPECT_EQ(insert_idx, boost::none); + EXPECT_EQ(insert_idx, std::nullopt); } // Empty @@ -4398,13 +4399,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); @@ -4424,13 +4425,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx + 1); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx + 1); EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); @@ -4450,13 +4451,13 @@ TEST(trajectory, insertDecelPoint_from_a_point) const size_t base_idx = findNearestSegmentIndex(traj.points, p_target); const auto insert_idx = insertDecelPoint(src_point, x_start, decel_velocity, traj_out.points); - EXPECT_NE(insert_idx, boost::none); - EXPECT_EQ(insert_idx.get(), base_idx); + EXPECT_NE(insert_idx, std::nullopt); + EXPECT_EQ(insert_idx.value(), base_idx); EXPECT_EQ(traj_out.points.size(), traj.points.size()); for (size_t i = 0; i < traj_out.points.size() - 1; ++i) { EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3); - if (i < insert_idx.get()) { + if (i < insert_idx.value()) { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), 10.0, epsilon); } else { EXPECT_NEAR(getLongitudinalVelocity(traj_out.points.at(i)), decel_velocity, epsilon); diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp index 6106e3a15f4c7..63cf6305e8158 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/geometry.hpp @@ -462,7 +462,8 @@ bool isDrivingForward(const Pose1 & src_pose, const Pose2 & dst_pose) * pose. */ geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z); + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z, + const double yaw = 0.0); /** * @brief Calculate a point by linear interpolation. diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp index 1d72b49a553d1..c00c3c1c364df 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/marker_helper.hpp @@ -19,8 +19,7 @@ #include -#include - +#include #include namespace tier4_autoware_utils @@ -75,7 +74,7 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time = {}); + const std::optional & current_time = {}); } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp index ab3ca18097e54..efb5564bdaa71 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp @@ -17,6 +17,9 @@ #include +#include + +#include #include #include @@ -40,6 +43,21 @@ inline std::string toHexString(const unique_identifier_msgs::msg::UUID & id) } return ss.str(); } + +inline boost::uuids::uuid toBoostUUID(const unique_identifier_msgs::msg::UUID & id) +{ + boost::uuids::uuid boost_uuid{}; + std::copy(id.uuid.begin(), id.uuid.end(), boost_uuid.begin()); + return boost_uuid; +} + +inline unique_identifier_msgs::msg::UUID toUUIDMsg(const boost::uuids::uuid & id) +{ + unique_identifier_msgs::msg::UUID ros_uuid{}; + std::copy(id.begin(), id.end(), ros_uuid.uuid.begin()); + return ros_uuid; +} + } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index e37cd0cc33975..04a07b88edf4e 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -18,7 +18,6 @@ builtin_interfaces diagnostic_msgs geometry_msgs - libboost-dev logging_demo pcl_conversions pcl_ros diff --git a/common/tier4_autoware_utils/src/geometry/geometry.cpp b/common/tier4_autoware_utils/src/geometry/geometry.cpp index 2c2de2d07a3dc..b88646b73dd7c 100644 --- a/common/tier4_autoware_utils/src/geometry/geometry.cpp +++ b/common/tier4_autoware_utils/src/geometry/geometry.cpp @@ -327,12 +327,13 @@ double calcCurvature( * pose. */ geometry_msgs::msg::Pose calcOffsetPose( - const geometry_msgs::msg::Pose & p, const double x, const double y, const double z) + const geometry_msgs::msg::Pose & p, const double x, const double y, const double z, + const double yaw) { geometry_msgs::msg::Pose pose; geometry_msgs::msg::Transform transform; transform.translation = createTranslation(x, y, z); - transform.rotation = createQuaternion(0.0, 0.0, 0.0, 1.0); + transform.rotation = createQuaternionFromYaw(yaw); tf2::Transform tf_pose; tf2::Transform tf_offset; tf2::fromMsg(transform, tf_offset); @@ -378,6 +379,7 @@ std::optional intersect( geometry_msgs::msg::Point intersect_point; intersect_point.x = t * p1.x + (1.0 - t) * p2.x; intersect_point.y = t * p1.y + (1.0 - t) * p2.y; + intersect_point.z = t * p1.z + (1.0 - t) * p2.z; return intersect_point; } diff --git a/common/tier4_autoware_utils/src/ros/marker_helper.cpp b/common/tier4_autoware_utils/src/ros/marker_helper.cpp index fe3a579e41d19..fda997edc83db 100644 --- a/common/tier4_autoware_utils/src/ros/marker_helper.cpp +++ b/common/tier4_autoware_utils/src/ros/marker_helper.cpp @@ -57,13 +57,13 @@ visualization_msgs::msg::Marker createDeletedDefaultMarker( void appendMarkerArray( const visualization_msgs::msg::MarkerArray & additional_marker_array, visualization_msgs::msg::MarkerArray * marker_array, - const boost::optional & current_time) + const std::optional & current_time) { for (const auto & marker : additional_marker_array.markers) { marker_array->markers.push_back(marker); if (current_time) { - marker_array->markers.back().header.stamp = current_time.get(); + marker_array->markers.back().header.stamp = current_time.value(); } } } diff --git a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp index ce91614155491..c10e04736b5bb 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_geometry.cpp @@ -1138,6 +1138,7 @@ TEST(geometry, calcOffsetPose) using tier4_autoware_utils::createQuaternionFromRPY; using tier4_autoware_utils::deg2rad; + // Only translation { geometry_msgs::msg::Pose p_in; p_in.position = createPoint(1.0, 2.0, 3.0); @@ -1185,6 +1186,40 @@ TEST(geometry, calcOffsetPose) EXPECT_DOUBLE_EQ(p_out.orientation.z, 0.25881904510252068); EXPECT_DOUBLE_EQ(p_out.orientation.w, 0.96592582628906831); } + + // Only rotation + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 0.0, 0.0, 0.0, deg2rad(20)); + + EXPECT_DOUBLE_EQ(p_out.position.x, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.y, 1.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 1.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); + EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); + } + + // Both translation and rotation + { + geometry_msgs::msg::Pose p_in; + p_in.position = createPoint(2.0, 1.0, 1.0); + p_in.orientation = createQuaternionFromRPY(deg2rad(0), deg2rad(0), deg2rad(30)); + + const auto p_out = calcOffsetPose(p_in, 2.0, 0.0, -1.0, deg2rad(20)); + + EXPECT_DOUBLE_EQ(p_out.position.x, 3.73205080756887729); + EXPECT_DOUBLE_EQ(p_out.position.y, 2.0); + EXPECT_DOUBLE_EQ(p_out.position.z, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(p_out.orientation.y, 0.0); + EXPECT_NEAR(p_out.orientation.z, 0.42261826174069944, epsilon); + EXPECT_NEAR(p_out.orientation.w, 0.9063077870366499, epsilon); + } } TEST(geometry, isDrivingForward) diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml index 23a5123d8d8e2..11dd050658aeb 100644 --- a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml +++ b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml @@ -85,3 +85,24 @@ Control: logger_name: control.vehicle_cmd_gate - node_name: /control/vehicle_cmd_gate logger_name: tier4_autoware_utils + +# ============================================================ +# common +# ============================================================ + +Common: + tier4_autoware_utils: + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/lane_driving/motion_planning/path_smoother + logger_name: tier4_autoware_utils + - node_name: /planning/scenario_planning/motion_velocity_smoother + logger_name: tier4_autoware_utils + - node_name: /control/trajectory_follower/controller_node_exe + logger_name: tier4_autoware_utils + - node_name: /control/vehicle_cmd_gate + logger_name: tier4_autoware_utils diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp index 3e95afe247c86..e27fc9b6cfa81 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp @@ -107,62 +107,9 @@ void VelocitySteeringFactorsPanel::onVelocityFactors(const VelocityFactorArray:: for (std::size_t i = 0; i < msg->factors.size(); i++) { const auto & e = msg->factors.at(i); - // type + // behavior { - auto label = new QLabel(); - switch (e.type) { - case VelocityFactor::SURROUNDING_OBSTACLE: - label->setText("SURROUNDING_OBSTACLE"); - break; - case VelocityFactor::ROUTE_OBSTACLE: - label->setText("ROUTE_OBSTACLE"); - break; - case VelocityFactor::INTERSECTION: - label->setText("INTERSECTION"); - break; - case VelocityFactor::CROSSWALK: - label->setText("CROSSWALK"); - break; - case VelocityFactor::REAR_CHECK: - label->setText("REAR_CHECK"); - break; - case VelocityFactor::USER_DEFINED_DETECTION_AREA: - label->setText("USER_DEFINED_DETECTION_AREA"); - break; - case VelocityFactor::NO_STOPPING_AREA: - label->setText("NO_STOPPING_AREA"); - break; - case VelocityFactor::STOP_SIGN: - label->setText("STOP_SIGN"); - break; - case VelocityFactor::TRAFFIC_SIGNAL: - label->setText("TRAFFIC_SIGNAL"); - break; - case VelocityFactor::V2I_GATE_CONTROL_ENTER: - label->setText("V2I_GATE_CONTROL_ENTER"); - break; - case VelocityFactor::V2I_GATE_CONTROL_LEAVE: - label->setText("V2I_GATE_CONTROL_LEAVE"); - break; - case VelocityFactor::MERGE: - label->setText("MERGE"); - break; - case VelocityFactor::SIDEWALK: - label->setText("SIDEWALK"); - break; - case VelocityFactor::LANE_CHANGE: - label->setText("LANE_CHANGE"); - break; - case VelocityFactor::AVOIDANCE: - label->setText("AVOIDANCE"); - break; - case VelocityFactor::EMERGENCY_STOP_OPERATION: - label->setText("EMERGENCY_STOP_OPERATION"); - break; - default: - label->setText("UNKNOWN"); - break; - } + auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str()); label->setAlignment(Qt::AlignCenter); velocity_factors_table_->setCellWidget(i, 0, label); } @@ -213,38 +160,9 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray:: for (std::size_t i = 0; i < msg->factors.size(); i++) { const auto & e = msg->factors.at(i); - // type + // behavior { - auto label = new QLabel(); - switch (e.type) { - case SteeringFactor::INTERSECTION: - label->setText("INTERSECTION"); - break; - case SteeringFactor::LANE_CHANGE: - label->setText("LANE_CHANGE"); - break; - case SteeringFactor::AVOIDANCE_PATH_CHANGE: - label->setText("AVOIDANCE_PATH_CHANGE"); - break; - case SteeringFactor::AVOIDANCE_PATH_RETURN: - label->setText("AVOIDANCE_PATH_RETURN"); - break; - case SteeringFactor::STATION: - label->setText("STATION"); - break; - case SteeringFactor::START_PLANNER: - label->setText("START_PLANNER"); - break; - case SteeringFactor::GOAL_PLANNER: - label->setText("GOAL_PLANNER"); - break; - case SteeringFactor::EMERGENCY_OPERATION: - label->setText("EMERGENCY_OPERATION"); - break; - default: - label->setText("UNKNOWN"); - break; - } + auto label = new QLabel(e.behavior.empty() ? "UNKNOWN" : e.behavior.c_str()); label->setAlignment(Qt::AlignCenter); steering_factors_table_->setCellWidget(i, 0, label); } diff --git a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp index fe867964239af..70f35ed3793ab 100644 --- a/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp +++ b/common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.hpp @@ -26,6 +26,7 @@ #include #include +#include #include #include @@ -35,6 +36,7 @@ namespace rviz_plugins { class VelocitySteeringFactorsPanel : public rviz_common::Panel { + using PlanningBehavior = autoware_adapi_v1_msgs::msg::PlanningBehavior; using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; using VelocityFactor = autoware_adapi_v1_msgs::msg::VelocityFactor; using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; diff --git a/common/trtexec_vendor/CMakeLists.txt b/common/trtexec_vendor/CMakeLists.txt deleted file mode 100644 index 0e5eebc621e39..0000000000000 --- a/common/trtexec_vendor/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(trtexec_vendor) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) - set(CMAKE_CXX_STANDARD_REQUIRED ON) - set(CMAKE_CXX_EXTENSIONS OFF) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(CUDA) -find_package(cudnn_cmake_module REQUIRED) -find_package(CUDNN) -find_package(tensorrt_cmake_module REQUIRED) -find_package(TENSORRT) - -if(NOT (CUDA_FOUND AND CUDNN_FOUND AND TENSORRT_FOUND)) - message(WARNING "cuda, cudnn, tensorrt libraries are not found") - return() -endif() - -if(TENSORRT_VERSION VERSION_LESS 8.2.1) - message(WARNING "The tensorrt version less than 8.2.1 isn't supported.") - return() -endif() - -set(TRTEXEC_DEFAULT_BIN /usr/src/tensorrt/bin/trtexec) -if(NOT EXISTS TRTEXEC_DEFAULT_BIN) - include(FetchContent) - fetchcontent_declare(tensorrt - GIT_REPOSITORY https://github.com/NVIDIA/TensorRT - GIT_TAG release/${TENSORRT_VERSION_MAJOR}.${TENSORRT_VERSION_MINOR} - GIT_SUBMODULES "" - ) - fetchcontent_getproperties(tensorrt) - if(NOT tensorrt_POPULATED) - fetchcontent_populate(tensorrt) - endif() - set(TRTEXEC_SOURCES - ${tensorrt_SOURCE_DIR}/samples/trtexec/trtexec.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleEngines.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleInference.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleOptions.cpp - ${tensorrt_SOURCE_DIR}/samples/common/sampleReporting.cpp - ${tensorrt_SOURCE_DIR}/samples/common/logger.cpp - ) - if(TENSORRT_VERSION VERSION_GREATER_EQUAL 8.4) - list(APPEND TRTEXEC_SOURCES - ${tensorrt_SOURCE_DIR}/samples/common/sampleUtils.cpp - ) - endif() - cuda_add_executable(${PROJECT_NAME} - ${TRTEXEC_SOURCES} - ) - target_link_libraries(${PROJECT_NAME} - ${TENSORRT_LIBRARIES} - ) - target_include_directories(${PROJECT_NAME} - PRIVATE ${tensorrt_SOURCE_DIR}/samples/common - ) - - set_target_properties(${PROJECT_NAME} - PROPERTIES OUTPUT_NAME trtexec - ) - - install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - ) -endif() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.sh.in") - -ament_package() diff --git a/common/trtexec_vendor/README.md b/common/trtexec_vendor/README.md deleted file mode 100644 index aec49936fd99e..0000000000000 --- a/common/trtexec_vendor/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# trtexec_vendor - -## Purpose - -This is a vendor package for trtexec. -It can build tensorrt engine from onnx file with trtexec. - -e.g. - -```bash -trtexec --onnx=(ros2 pkg prefix tensorrt_yolox --share)/data/yolox-tiny.onnx -``` diff --git a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in b/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in deleted file mode 100644 index 7c2ef456ecfec..0000000000000 --- a/common/trtexec_vendor/env-hooks/trtexec_vendor.sh.in +++ /dev/null @@ -1,5 +0,0 @@ -TRTEXEC_DEFAULT_BIN_DIR=/usr/src/tensorrt/bin -TRTEXEC_DEFAULT_BIN=$TRTEXEC_DEFAULT_BIN_DIR/trtexec -if [ -f $TRTEXEC_DEFAULT_BIN ]; then - ament_prepend_unique_value PATH $TRTEXEC_DEFAULT_BIN_DIR -fi diff --git a/common/trtexec_vendor/package.xml b/common/trtexec_vendor/package.xml deleted file mode 100644 index 1fc6c928e3ec5..0000000000000 --- a/common/trtexec_vendor/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - trtexec_vendor - 0.1.0 - The vendor package of trtexec - Daisuke Nishimatsu - Yusuke Muramatsu - Apache 2.0 - - ament_cmake - cudnn_cmake_module - tensorrt_cmake_module - - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp index 275be99f418f1..6bf8fb1c34f5e 100644 --- a/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp +++ b/control/control_performance_analysis/include/control_performance_analysis/control_performance_analysis_core.hpp @@ -33,6 +33,7 @@ #include #include +#include #include #include @@ -79,7 +80,7 @@ class ControlPerformanceAnalysisCore void setOdomHistory(const Odometry & odom); void setSteeringStatus(const SteeringReport & steering); - boost::optional findCurveRefIdx(); + std::optional findCurveRefIdx(); std::pair findClosestPrevWayPointIdx_path_direction(); double estimateCurvature(); double estimatePurePursuitCurvature(); diff --git a/control/control_performance_analysis/src/control_performance_analysis_core.cpp b/control/control_performance_analysis/src/control_performance_analysis_core.cpp index 777f5dee3e470..1ceef81e10c56 100644 --- a/control/control_performance_analysis/src/control_performance_analysis_core.cpp +++ b/control/control_performance_analysis/src/control_performance_analysis_core.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -93,8 +94,8 @@ std::pair ControlPerformanceAnalysisCore::findClosestPrevWayPoint return std::make_pair(false, std::numeric_limits::quiet_NaN()); } - idx_prev_wp_ = std::make_unique(closest_segment.get()); - idx_next_wp_ = std::make_unique(closest_segment.get() + 1); + idx_prev_wp_ = std::make_unique(closest_segment.value()); + idx_next_wp_ = std::make_unique(closest_segment.value() + 1); return std::make_pair(true, *idx_prev_wp_); } @@ -360,13 +361,13 @@ void ControlPerformanceAnalysisCore::setSteeringStatus(const SteeringReport & st current_vec_steering_msg_ptr_ = std::make_shared(steering); } -boost::optional ControlPerformanceAnalysisCore::findCurveRefIdx() +std::optional ControlPerformanceAnalysisCore::findCurveRefIdx() { // Get the previous waypoint as the reference if (!interpolated_pose_ptr_) { RCLCPP_WARN_THROTTLE( logger_, clock_, 1000, "Cannot set the curvature_idx, no valid interpolated pose ..."); - return boost::none; + return std::nullopt; } auto fun_distance_cond = [this](auto point_t) { @@ -428,7 +429,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature() RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint "); return 0; } - const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.get(); + const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.value(); const auto & points = current_trajectory_ptr_->points; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index b40a0d0473135..06d11133920f4 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -53,13 +53,13 @@ std::array triangle2points( return points; } -std::map getRouteLanelets( +std::map getRouteLanelets( const lanelet::LaneletMapPtr & lanelet_map, const lanelet::routing::RoutingGraphPtr & routing_graph, const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr, const double vehicle_length) { - std::map route_lanelets; + std::map route_lanelets; bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map); if (!is_route_valid) { @@ -315,7 +315,7 @@ void LaneDepartureCheckerNode::onTimer() // In order to wait for both of map and route will be ready, write this not in callback but here if (last_route_ != route_ && !route_->segments.empty()) { - std::map::iterator itr; + std::map::iterator itr; auto map_route_lanelets_ = getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_); diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index 50e9d44a0a7a4..6bfba5818bd06 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -41,9 +41,7 @@ For the optimization, a Quadratic Programming (QP) solver is used and two option ### Filtering Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as -input of the MPC as well as for -the output steering angle. +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is employed for processing the yaw and lateral errors, which are used as inputs for the MPC, as well as for refining the output steering angle. Other filtering methods can be considered as long as the noise reduction performances are good enough. The moving average filter for example is not suited and can yield worse results than without any diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 6bd885b78beb7..153c2ae61d076 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -204,7 +204,7 @@ void MPC::setReferenceTrajectory( motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints()); // if driving direction is unknown, use previous value - m_is_forward_shift = is_forward_shift ? is_forward_shift.get() : m_is_forward_shift; + m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift; // path smoothing MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled; // smooth filtered trajectory diff --git a/control/obstacle_collision_checker/schema/obstacle_collision_checker.json b/control/obstacle_collision_checker/schema/obstacle_collision_checker.json new file mode 100644 index 0000000000000..d1ba379a732df --- /dev/null +++ b/control/obstacle_collision_checker/schema/obstacle_collision_checker.json @@ -0,0 +1,60 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for obstacle collision checker", + "type": "object", + "definitions": { + "vehicle_cmd_gate": { + "type": "object", + "properties": { + "delay_time": { + "type": "number", + "default": 0.3, + "description": "Delay time of vehicles." + }, + "update_rate": { + "type": "number" + }, + "footprint_margin": { + "type": "number", + "default": 0.0, + "description": "Foot print margin." + }, + "max_deceleration": { + "type": "number", + "default": 2.0, + "description": "Max deceleration for ego vehicle to stop." + }, + "resample_interval": { + "type": "number", + "default": 0.3, + "description": "Interval for resampling trajectory." + }, + "search_radius": { + "type": "number", + "default": 5.0, + "description": "Search distance from trajectory to point cloud" + } + }, + "required": [ + "delay_time", + "footprint_margin", + "max_deceleration", + "resample_interval", + "search_radius", + "update_rate" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/obstacle_collision_checker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/operation_mode_transition_manager/README.md b/control/operation_mode_transition_manager/README.md index 3cb07a9bd7821..ea0688e88d9f1 100644 --- a/control/operation_mode_transition_manager/README.md +++ b/control/operation_mode_transition_manager/README.md @@ -83,6 +83,8 @@ For the backward compatibility (to be removed): ## Parameters +{{ json_to_markdown("control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json") }} + | Name | Type | Description | Default value | | :--------------------------------- | :------- | :---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | | `transition_timeout` | `double` | If the state transition is not completed within this time, it is considered a transition failure. | 10.0 | diff --git a/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json b/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json new file mode 100644 index 0000000000000..8ce8eda4c6c1c --- /dev/null +++ b/control/operation_mode_transition_manager/schema/operation_mode_transition_manager.schema.json @@ -0,0 +1,168 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Operation Mode Transition Manager Node", + "type": "object", + "definitions": { + "operation_mode_transition_manager": { + "type": "object", + "properties": { + "transition_timeout": { + "type": "number", + "description": "If the state transition is not completed within this time, it is considered a transition failure.", + "default": "10.0", + "minimum": 0.0 + }, + "frequency_hz": { + "type": "number", + "description": "running hz", + "default": "10.0", + "minimum": 0.0 + }, + "enable_engage_on_driving": { + "type": "boolean", + "description": "Set true if you want to engage the autonomous driving mode while the vehicle is driving. If set to false, it will deny Engage in any situation where the vehicle speed is not zero. Note that if you use this feature without adjusting the parameters, it may cause issues like sudden deceleration. Before using, please ensure the engage condition and the vehicle_cmd_gate transition filter are appropriately adjusted.", + "default": "false" + }, + "check_engage_condition": { + "type": "boolean", + "description": "If false, autonomous transition is always available.", + "default": "false" + }, + "nearest_dist_deviation_threshold": { + "type": "number", + "description": "distance threshold used to find nearest trajectory point [m]", + "default": "3.0", + "minimum": 0.0 + }, + "nearest_yaw_deviation_threshold": { + "type": "number", + "description": "angle threshold used to find nearest trajectory point [rad]", + "default": "1.57", + "minimum": -3.142 + }, + "engage_acceptable_limits": { + "type": "object", + "properties": { + "allow_autonomous_in_stopped": { + "type": "boolean", + "description": "If true, autonomous transition is available when the vehicle is stopped even if other checks fail.", + "default": "true" + }, + "dist_threshold": { + "type": "number", + "description": "The distance between the trajectory and ego vehicle must be within this distance for Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "yaw_threshold": { + "type": "number", + "description": "The yaw angle between trajectory and ego vehicle must be within this threshold for Autonomous transition.", + "default": "0.524", + "minimum": -3.142 + }, + "speed_upper_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold for Autonomous transition.", + "default": "10.0" + }, + "speed_lower_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold for Autonomous transition.", + "default": "-10.0" + }, + "acc_threshold": { + "type": "number", + "description": "The control command acceleration must be less than this threshold for Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "lateral_acc_threshold": { + "type": "number", + "description": "The control command lateral acceleration must be less than this threshold for Autonomous transition.", + "default": "1.0", + "minimum": 0.0 + }, + "lateral_acc_diff_threshold": { + "type": "number", + "description": "The lateral acceleration deviation between the control command must be less than this threshold for Autonomous transition.", + "default": "0.5", + "minimum": 0.0 + } + }, + "required": [ + "allow_autonomous_in_stopped", + "dist_threshold", + "yaw_threshold", + "speed_upper_threshold", + "speed_lower_threshold", + "acc_threshold", + "lateral_acc_threshold", + "lateral_acc_diff_threshold" + ] + }, + "stable_check": { + "type": "object", + "properties": { + "duration": { + "type": "number", + "description": "The stable condition must be satisfied for this duration to complete the transition.", + "default": "0.1", + "minimum": 0.0 + }, + "dist_threshold": { + "type": "number", + "description": "The distance between the trajectory and ego vehicle must be within this distance to complete Autonomous transition.", + "default": "1.5", + "minimum": 0.0 + }, + "speed_upper_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "2.0" + }, + "speed_lower_threshold": { + "type": "number", + "description": "The velocity deviation between control command and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "-2.0" + }, + "yaw_threshold": { + "type": "number", + "description": "The yaw angle between trajectory and ego vehicle must be within this threshold to complete Autonomous transition.", + "default": "0,262", + "minimum": -3.142 + } + }, + "required": [ + "duration", + "dist_threshold", + "speed_upper_threshold", + "speed_lower_threshold", + "yaw_threshold" + ] + } + }, + "required": [ + "transition_timeout", + "frequency_hz", + "enable_engage_on_driving", + "check_engage_condition", + "nearest_dist_deviation_threshold", + "nearest_yaw_deviation_threshold", + "engage_acceptable_limits", + "stable_check" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/operation_mode_transition_manager" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 9d25322c793e0..a110cb94d1d91 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -90,8 +90,13 @@ This PID logic has a maximum value for the output of each term. This is to preve - Large integral terms may cause unintended behavior by users. - Unintended noise may cause the output of the derivative term to be very large. -Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. -On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. +Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. + +However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. + +When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. + +The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. This may be replaced by a higher performance controller (adaptive control or robust control) in future development. @@ -207,7 +212,9 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_d_effort | double | max value of acceleration with d gain | 0.0 | | min_d_effort | double | min value of acceleration with d gain | 0.0 | | lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | -| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 | +| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | +| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | +| time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s] | 5.0 | | brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | ### STOPPING Parameter (smooth stop) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 921cd3995f6a9..b4ac9e1506eb7 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -97,6 +98,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // vehicle info double m_wheel_base; + bool m_prev_vehicle_is_under_control{false}; + std::shared_ptr m_under_control_starting_time{nullptr}; // control state enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; @@ -145,7 +148,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // drive PIDController m_pid_vel; std::shared_ptr m_lpf_vel_error{nullptr}; + bool m_enable_integration_at_low_speed; double m_current_vel_threshold_pid_integrate; + double m_time_threshold_before_pid_integrate; bool m_enable_brake_keeping_before_stop; double m_brake_keeping_acc; @@ -384,6 +389,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro void updateDebugVelAcc( const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); + + double getTimeUnderControl(); }; } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index eb2ef443c4576..f1ddea7ebad88 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index d13e628f2e1d4..12f0eece1e477 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -100,9 +100,14 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) const double lpf_vel_error_gain{node.declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); + m_enable_integration_at_low_speed = + node.declare_parameter("enable_integration_at_low_speed"); m_current_vel_threshold_pid_integrate = node.declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + m_time_threshold_before_pid_integrate = + node.declare_parameter("time_threshold_before_pid_integration"); // [s] + m_enable_brake_keeping_before_stop = node.declare_parameter("enable_brake_keeping_before_stop"); // [-] m_brake_keeping_acc = node.declare_parameter("brake_keeping_acc"); // [m/s^2] @@ -284,6 +289,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); + update_param("time_threshold_before_pid_integration", m_time_threshold_before_pid_integrate); } // stopping state @@ -556,6 +562,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + if (is_under_control != m_prev_vehicle_is_under_control) { + m_prev_vehicle_is_under_control = is_under_control; + m_under_control_starting_time = + is_under_control ? std::make_shared(clock_->now()) : nullptr; + } // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { @@ -959,8 +970,15 @@ double PidLongitudinalController::applyVelocityFeedback( const double diff_vel = (target_motion.vel - current_vel) * vel_sign; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + + const bool vehicle_is_moving = std::abs(current_vel) > m_current_vel_threshold_pid_integrate; + const double time_under_control = getTimeUnderControl(); + const bool vehicle_is_stuck = + !vehicle_is_moving && time_under_control > m_time_threshold_before_pid_integrate; + const bool enable_integration = - (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + (vehicle_is_moving || (m_enable_integration_at_low_speed && vehicle_is_stuck)) && + is_under_control; const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); @@ -1059,4 +1077,10 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } +double PidLongitudinalController::getTimeUnderControl() +{ + if (!m_under_control_starting_time) return 0.0; + return (clock_->now() - *m_under_control_starting_time).seconds(); +} + } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/shift_decider/schema/shift_decider.schema.json b/control/shift_decider/schema/shift_decider.schema.json new file mode 100644 index 0000000000000..faba3c4e12064 --- /dev/null +++ b/control/shift_decider/schema/shift_decider.schema.json @@ -0,0 +1,30 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Shift Decider Node", + "type": "object", + "definitions": { + "shift_decider": { + "type": "object", + "properties": { + "park_on_goal": { + "type": "boolean", + "description": "Setting true to part on goal.", + "default": "true" + } + }, + "required": ["park_on_goal"] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/shift_decider" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml index bc3213081d86e..c39088753fe64 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2 diff --git a/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json b/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json new file mode 100644 index 0000000000000..983a001922053 --- /dev/null +++ b/control/vehicle_cmd_gate/schema/vehicle_cmd_gate.json @@ -0,0 +1,104 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for vehicle cmd gate", + "type": "object", + "definitions": { + "vehicle_cmd_gate": { + "type": "object", + "properties": { + "update_rate": { + "type": "number", + "default": 10.0, + "description": "To update period" + }, + "use_emergency_handling": { + "type": "boolean", + "default": "false", + "description": "true when emergency handler is used." + }, + "use_external_emergency_stop": { + "type": "boolean", + "default": "false", + "description": "true when external emergency stop information is used." + }, + "system_emergency_heartbeat_timeout": { + "type": "number", + "default": 0.5, + "description": "timeout for system emergency." + }, + "external_emergency_stop_heartbeat_timeout": { + "type": "number", + "default": 0.5, + "description": "timeout for external emergency." + }, + "stop_hold_acceleration": { + "type": "number", + "default": -1.5, + "description": "longitudinal acceleration cmd when vehicle should stop" + }, + "emergency_acceleration": { + "type": "number", + "default": -2.4, + "description": "longitudinal acceleration cmd when vehicle stop with emergency." + }, + "nominal.vel_lim": { + "type": "number", + "default": 25.0, + "description": "limit of longitudinal velocity (activated in AUTONOMOUS operation mode)." + }, + "nominal.lon_acc_lim": { + "type": "number", + "default": 5.0, + "description": "limit of longitudinal acceleration (activated in AUTONOMOUS operation mode)." + }, + "nominal.lon_jerk_lim": { + "type": "number", + "default": 5.0, + "description": "limit of longitudinal jerk (activated in AUTONOMOUS operation mode)." + }, + "nominal.lat_acc_lim": { + "type": "number", + "default": 5.0, + "description": "limit of lateral acceleration (activated in AUTONOMOUS operation mode)." + }, + "nominal.lat_jerk_lim": { + "type": "number", + "default": 5.0, + "description": "limit of lateral jerk (activated in AUTONOMOUS operation mode)." + }, + "nominal.actual_steer_diff_lim": { + "type": "number", + "default": 1.0, + "description": "limit of actual steer diff (activated in AUTONOMOUS operation mode)." + } + }, + "required": [ + "update_rate", + "use_emergency_handling", + "use_external_emergency_stop", + "system_emergency_heartbeat_timeout", + "external_emergency_stop_heartbeat_timeout", + "stop_hold_acceleration", + "emergency_acceleration", + "nominal.vel_lim", + "nominal.lon_acc_lim", + "nominal.lon_jerk_lim", + "nominal.lat_acc_lim", + "nominal.lat_jerk_lim", + "nominal.actual_steer_diff_lim" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/vehicle_cmd_gate" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index b3e9e9148ee60..227aac50d6d90 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -7,10 +7,8 @@ - - @@ -194,8 +192,9 @@ - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 1b4c3f38e038d..65caad54876ff 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -218,8 +218,9 @@ - - + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index b69f1c0ee8b14..0d03e3c8cdcd9 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -70,11 +70,9 @@ - - + - @@ -106,7 +104,6 @@ - @@ -120,7 +117,6 @@ - @@ -136,7 +132,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index ac521934265d7..2c8b7d6581c53 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -60,8 +60,9 @@ - - + + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 6582dfd32d91d..96ad80131e4e7 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -177,7 +177,6 @@ - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index ba2318c438674..3a1f417106f54 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -16,6 +16,17 @@ + + + + + + + + + + + @@ -32,6 +43,60 @@ + + + + + + + + + + + + + + - - - - - - - - - - + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 0870a384ea7d3..789cf136f1152 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -5,7 +5,7 @@ 0.1.0 The tier4_system_launch package Fumihito Ito - asana17 + TetsuKawa Apache License 2.0 ament_cmake_auto diff --git a/localization/landmark_based_localizer/README.md b/localization/landmark_based_localizer/README.md index e4c79b7941b12..86f3750ad11d9 100644 --- a/localization/landmark_based_localizer/README.md +++ b/localization/landmark_based_localizer/README.md @@ -105,3 +105,20 @@ For example, when using the AR tag, it would look like this. ... ``` + +## About `consider_orientation` + +The `calculate_new_self_pose` function in the `LandmarkManager` class includes a boolean argument named `consider_orientation`. This argument determines the method used to calculate the new self pose based on detected and mapped landmarks. The following image illustrates the difference between the two methods. + +![consider_orientation_figure](./doc_image/consider_orientation.drawio.svg) + +### `consider_orientation = true` + +In this mode, the new self pose is calculated so that the relative Pose of the "landmark detected from the current self pose" is equal to the relative Pose of the "landmark mapped from the new self pose". +This method can correct for orientation, but is strongly affected by the orientation error of the landmark detection. + +### `consider_orientation = false` + +In this mode, the new self pose is calculated so that only the relative position is correct for x, y, and z. + +This method can not correct for orientation, but it is not affected by the orientation error of the landmark detection. diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml index 29260e27cd43c..e8f90165376ce 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/config/ar_tag_based_localizer.param.yaml @@ -18,6 +18,9 @@ # Detect AR-Tags within this range and publish the pose of ego vehicle distance_threshold: 13.0 # [m] + # consider_orientation + consider_orientation: false + # Detector parameters # See https://github.com/pal-robotics/aruco_ros/blob/7787a6794d30c248bc546d1582e65dd47bc40c12/aruco/include/aruco/markerdetector.h#L106-L126 detection_mode: "DM_NORMAL" # select from [DM_NORMAL, DM_FAST, DM_VIDEO_FAST] diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp index a66277c699a00..e569a71bbb5b0 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.cpp @@ -49,17 +49,21 @@ #include #include #include +#include #include #include #include +#include #ifdef ROS_DISTRO_GALACTIC #include #else #include #endif +#include + ArTagBasedLocalizer::ArTagBasedLocalizer(const rclcpp::NodeOptions & options) : Node("ar_tag_based_localizer", options), cam_info_received_(false) { @@ -73,7 +77,8 @@ bool ArTagBasedLocalizer::setup() marker_size_ = static_cast(this->declare_parameter("marker_size")); target_tag_ids_ = this->declare_parameter>("target_tag_ids"); base_covariance_ = this->declare_parameter>("base_covariance"); - distance_threshold_squared_ = std::pow(this->declare_parameter("distance_threshold"), 2); + distance_threshold_ = this->declare_parameter("distance_threshold"); + consider_orientation_ = this->declare_parameter("consider_orientation"); ekf_time_tolerance_ = this->declare_parameter("ekf_time_tolerance"); ekf_position_tolerance_ = this->declare_parameter("ekf_position_tolerance"); std::string detection_mode = this->declare_parameter("detection_mode"); @@ -89,6 +94,8 @@ bool ArTagBasedLocalizer::setup() RCLCPP_ERROR_STREAM(this->get_logger(), "Invalid detection_mode: " << detection_mode); return false; } + ekf_pose_buffer_ = std::make_unique( + this->get_logger(), ekf_time_tolerance_, ekf_position_tolerance_); /* Log parameter info @@ -103,7 +110,6 @@ bool ArTagBasedLocalizer::setup() */ tf_buffer_ = std::make_unique(this->get_clock()); tf_listener_ = std::make_unique(*tf_buffer_); - tf_broadcaster_ = std::make_unique(this); /* Initialize image transport @@ -119,6 +125,7 @@ bool ArTagBasedLocalizer::setup() rclcpp::QoS qos_sub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); qos_sub.best_effort(); + pose_array_pub_ = this->create_publisher("~/debug/detected_landmarks", qos_sub); image_sub_ = this->create_subscription( "~/input/image", qos_sub, std::bind(&ArTagBasedLocalizer::image_callback, this, std::placeholders::_1)); @@ -148,83 +155,85 @@ bool ArTagBasedLocalizer::setup() void ArTagBasedLocalizer::map_bin_callback(const HADMapBin::ConstSharedPtr & msg) { - const std::vector landmarks = - landmark_manager::parse_landmarks(msg, "apriltag_16h5", this->get_logger()); - for (const landmark_manager::Landmark & landmark : landmarks) { - landmark_map_[landmark.id] = landmark.pose; - } - - const MarkerArray marker_msg = landmark_manager::convert_landmarks_to_marker_array_msg(landmarks); + landmark_manager_.parse_landmarks(msg, "apriltag_16h5", this->get_logger()); + const MarkerArray marker_msg = landmark_manager_.get_landmarks_as_marker_array_msg(); marker_pub_->publish(marker_msg); } void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) { + // check subscribers if ((image_pub_.getNumSubscribers() == 0) && (pose_pub_->get_subscription_count() == 0)) { RCLCPP_DEBUG(this->get_logger(), "No subscribers, not looking for ArUco markers"); return; } + // check cam_info if (!cam_info_received_) { RCLCPP_DEBUG(this->get_logger(), "No cam_info has been received."); return; } - const builtin_interfaces::msg::Time curr_stamp = msg->header.stamp; - cv_bridge::CvImagePtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); - } catch (cv_bridge::Exception & e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; + + // get self pose + const std::optional interpolate_result = + ekf_pose_buffer_->interpolate(sensor_stamp); + if (!interpolate_result) { return; } + ekf_pose_buffer_->pop_old(sensor_stamp); + const Pose self_pose = interpolate_result.value().interpolated_pose.pose.pose; - cv::Mat in_image = cv_ptr->image; - - // detection results will go into "markers" - std::vector markers; - - // ok, let's detect - detector_.detect(in_image, markers, cam_param_, marker_size_, false); - - // for each marker, draw info and its boundaries in the image - for (const aruco::Marker & marker : markers) { - const tf2::Transform tf_cam_to_marker = aruco_marker_to_tf2(marker); + // detect + const std::vector landmarks = detect_landmarks(msg); + if (landmarks.empty()) { + return; + } - TransformStamped tf_cam_to_marker_stamped; - tf2::toMsg(tf_cam_to_marker, tf_cam_to_marker_stamped.transform); - tf_cam_to_marker_stamped.header.stamp = curr_stamp; - tf_cam_to_marker_stamped.header.frame_id = msg->header.frame_id; - tf_cam_to_marker_stamped.child_frame_id = "detected_marker_" + std::to_string(marker.id); - tf_broadcaster_->sendTransform(tf_cam_to_marker_stamped); + // for debug + if (pose_array_pub_->get_subscription_count() > 0) { + PoseArray pose_array_msg; + pose_array_msg.header.stamp = sensor_stamp; + pose_array_msg.header.frame_id = "map"; + for (const landmark_manager::Landmark & landmark : landmarks) { + const Pose detected_marker_on_map = + tier4_autoware_utils::transformPose(landmark.pose, self_pose); + pose_array_msg.poses.push_back(detected_marker_on_map); + } + pose_array_pub_->publish(pose_array_msg); + } - PoseStamped pose_cam_to_marker; - tf2::toMsg(tf_cam_to_marker, pose_cam_to_marker.pose); - pose_cam_to_marker.header.stamp = curr_stamp; - pose_cam_to_marker.header.frame_id = msg->header.frame_id; - publish_pose_as_base_link(pose_cam_to_marker, std::to_string(marker.id)); + // calc new_self_pose + const Pose new_self_pose = + landmark_manager_.calculate_new_self_pose(landmarks, self_pose, consider_orientation_); + const Pose diff_pose = tier4_autoware_utils::inverseTransformPose(new_self_pose, self_pose); + const double distance = + std::hypot(diff_pose.position.x, diff_pose.position.y, diff_pose.position.z); - // drawing the detected markers - marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + // check distance + if (distance > ekf_position_tolerance_) { + RCLCPP_INFO_STREAM(this->get_logger(), "distance: " << distance); + return; } - // draw a 3d cube in each marker if there is 3d info - if (cam_param_.isValid()) { - for (aruco::Marker & marker : markers) { - aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); - } - } + // publish + PoseWithCovarianceStamped pose_with_covariance_stamped; + pose_with_covariance_stamped.header.stamp = sensor_stamp; + pose_with_covariance_stamped.header.frame_id = "map"; + pose_with_covariance_stamped.pose.pose = new_self_pose; - if (image_pub_.getNumSubscribers() > 0) { - // show input with augmented information - cv_bridge::CvImage out_msg; - out_msg.header.stamp = curr_stamp; - out_msg.encoding = sensor_msgs::image_encodings::RGB8; - out_msg.image = in_image; - image_pub_.publish(out_msg.toImageMsg()); + // ~5[m]: base_covariance + // 5~[m]: scaling base_covariance by std::pow(distance / 5, 3) + const double coeff = std::max(1.0, std::pow(distance / 5, 3)); + for (int i = 0; i < 36; i++) { + pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; } - const int detected_tags = static_cast(markers.size()); + pose_pub_->publish(pose_with_covariance_stamped); + + // publish diagnostics + const int detected_tags = static_cast(landmarks.size()); diagnostic_msgs::msg::DiagnosticStatus diag_status; @@ -252,150 +261,113 @@ void ArTagBasedLocalizer::image_callback(const Image::ConstSharedPtr & msg) } // wait for one camera info, then shut down that subscriber -void ArTagBasedLocalizer::cam_info_callback(const CameraInfo & msg) +void ArTagBasedLocalizer::cam_info_callback(const CameraInfo::ConstSharedPtr & msg) { if (cam_info_received_) { return; } cv::Mat camera_matrix(3, 4, CV_64FC1, 0.0); - camera_matrix.at(0, 0) = msg.p[0]; - camera_matrix.at(0, 1) = msg.p[1]; - camera_matrix.at(0, 2) = msg.p[2]; - camera_matrix.at(0, 3) = msg.p[3]; - camera_matrix.at(1, 0) = msg.p[4]; - camera_matrix.at(1, 1) = msg.p[5]; - camera_matrix.at(1, 2) = msg.p[6]; - camera_matrix.at(1, 3) = msg.p[7]; - camera_matrix.at(2, 0) = msg.p[8]; - camera_matrix.at(2, 1) = msg.p[9]; - camera_matrix.at(2, 2) = msg.p[10]; - camera_matrix.at(2, 3) = msg.p[11]; + camera_matrix.at(0, 0) = msg->p[0]; + camera_matrix.at(0, 1) = msg->p[1]; + camera_matrix.at(0, 2) = msg->p[2]; + camera_matrix.at(0, 3) = msg->p[3]; + camera_matrix.at(1, 0) = msg->p[4]; + camera_matrix.at(1, 1) = msg->p[5]; + camera_matrix.at(1, 2) = msg->p[6]; + camera_matrix.at(1, 3) = msg->p[7]; + camera_matrix.at(2, 0) = msg->p[8]; + camera_matrix.at(2, 1) = msg->p[9]; + camera_matrix.at(2, 2) = msg->p[10]; + camera_matrix.at(2, 3) = msg->p[11]; cv::Mat distortion_coeff(4, 1, CV_64FC1); for (int i = 0; i < 4; ++i) { distortion_coeff.at(i, 0) = 0; } - const cv::Size size(static_cast(msg.width), static_cast(msg.height)); + const cv::Size size(static_cast(msg->width), static_cast(msg->height)); cam_param_ = aruco::CameraParameters(camera_matrix, distortion_coeff, size); cam_info_received_ = true; } -void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped & msg) +void ArTagBasedLocalizer::ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg) { - latest_ekf_pose_ = msg; + if (msg->header.frame_id == "map") { + ekf_pose_buffer_->push_back(msg); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << msg->header.frame_id << ", but expected map. " + << "Please check the frame_id in the input topic and ensure it is correct."); + } } -void ArTagBasedLocalizer::publish_pose_as_base_link( - const PoseStamped & sensor_to_tag, const std::string & tag_id) +std::vector ArTagBasedLocalizer::detect_landmarks( + const Image::ConstSharedPtr & msg) { - // Check tag_id - if (std::find(target_tag_ids_.begin(), target_tag_ids_.end(), tag_id) == target_tag_ids_.end()) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in target_tag_ids"); - return; - } - if (landmark_map_.count(tag_id) == 0) { - RCLCPP_INFO_STREAM(this->get_logger(), "tag_id(" << tag_id << ") is not in landmark_map_"); - return; - } + const builtin_interfaces::msg::Time sensor_stamp = msg->header.stamp; - // Range filter - const double distance_squared = sensor_to_tag.pose.position.x * sensor_to_tag.pose.position.x + - sensor_to_tag.pose.position.y * sensor_to_tag.pose.position.y + - sensor_to_tag.pose.position.z * sensor_to_tag.pose.position.z; - if (distance_threshold_squared_ < distance_squared) { - return; + // get image + cv::Mat in_image; + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::RGB8); + cv_ptr->image.copyTo(in_image); + } catch (cv_bridge::Exception & e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return std::vector{}; } - // Transform to base_link - PoseStamped base_link_to_tag; + // get transform from base_link to camera + TransformStamped transform_sensor_to_base_link; try { - const TransformStamped transform = - tf_buffer_->lookupTransform("base_link", sensor_to_tag.header.frame_id, tf2::TimePointZero); - tf2::doTransform(sensor_to_tag, base_link_to_tag, transform); - base_link_to_tag.header.frame_id = "base_link"; + transform_sensor_to_base_link = + tf_buffer_->lookupTransform("base_link", msg->header.frame_id, sensor_stamp); } catch (tf2::TransformException & ex) { RCLCPP_INFO(this->get_logger(), "Could not transform base_link to camera: %s", ex.what()); - return; + return std::vector{}; } - // (1) map_to_tag - const Pose & map_to_tag = landmark_map_.at(tag_id); - const Eigen::Affine3d map_to_tag_affine = pose_to_affine3d(map_to_tag); - - // (2) tag_to_base_link - const Eigen::Affine3d base_link_to_tag_affine = pose_to_affine3d(base_link_to_tag.pose); - const Eigen::Affine3d tag_to_base_link_affine = base_link_to_tag_affine.inverse(); - - // calculate map_to_base_link - const Eigen::Affine3d map_to_base_link_affine = map_to_tag_affine * tag_to_base_link_affine; - const Pose map_to_base_link = tf2::toMsg(map_to_base_link_affine); - - // If latest_ekf_pose_ is older than seconds compared to current frame, it - // will not be published. - const rclcpp::Duration diff_time = - rclcpp::Time(sensor_to_tag.header.stamp) - rclcpp::Time(latest_ekf_pose_.header.stamp); - if (diff_time.seconds() > ekf_time_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "latest_ekf_pose_ is older than %f seconds compared to current frame. " - "latest_ekf_pose_.header.stamp: %d.%d, sensor_to_tag.header.stamp: %d.%d", - ekf_time_tolerance_, latest_ekf_pose_.header.stamp.sec, latest_ekf_pose_.header.stamp.nanosec, - sensor_to_tag.header.stamp.sec, sensor_to_tag.header.stamp.nanosec); - return; - } - - // If curr_pose differs from latest_ekf_pose_ by more than , it will not - // be published. - const Pose curr_pose = map_to_base_link; - const Pose latest_ekf_pose = latest_ekf_pose_.pose.pose; - const double diff_position = norm(curr_pose.position, latest_ekf_pose.position); - if (diff_position > ekf_position_tolerance_) { - RCLCPP_INFO( - this->get_logger(), - "curr_pose differs from latest_ekf_pose_ by more than %f m. " - "curr_pose: (%f, %f, %f), latest_ekf_pose: (%f, %f, %f)", - ekf_position_tolerance_, curr_pose.position.x, curr_pose.position.y, curr_pose.position.z, - latest_ekf_pose.position.x, latest_ekf_pose.position.y, latest_ekf_pose.position.z); - return; - } + // detect + std::vector markers; + detector_.detect(in_image, markers, cam_param_, marker_size_, false); - // Construct output message - PoseWithCovarianceStamped pose_with_covariance_stamped; - pose_with_covariance_stamped.header.stamp = sensor_to_tag.header.stamp; - pose_with_covariance_stamped.header.frame_id = "map"; - pose_with_covariance_stamped.pose.pose = curr_pose; + // parse + std::vector landmarks; + + for (aruco::Marker & marker : markers) { + // convert marker pose to tf + const cv::Quat q = cv::Quat::createFromRvec(marker.Rvec); + Pose pose; + pose.position.x = marker.Tvec.at(0, 0); + pose.position.y = marker.Tvec.at(1, 0); + pose.position.z = marker.Tvec.at(2, 0); + pose.orientation.x = q.x; + pose.orientation.y = q.y; + pose.orientation.z = q.z; + pose.orientation.w = q.w; + const double distance = std::hypot(pose.position.x, pose.position.y, pose.position.z); + if (distance <= distance_threshold_) { + tf2::doTransform(pose, pose, transform_sensor_to_base_link); + landmarks.push_back(landmark_manager::Landmark{std::to_string(marker.id), pose}); + } - // ~5[m]: base_covariance - // 5~[m]: scaling base_covariance by std::pow(distance/5, 3) - const double distance = std::sqrt(distance_squared); - const double scale = distance / 5; - const double coeff = std::max(1.0, std::pow(scale, 3)); - for (int i = 0; i < 36; i++) { - pose_with_covariance_stamped.pose.covariance[i] = coeff * base_covariance_[i]; + // for debug, drawing the detected markers + marker.draw(in_image, cv::Scalar(0, 0, 255), 2); + aruco::CvDrawingUtils::draw3dAxis(in_image, marker, cam_param_); } - pose_pub_->publish(pose_with_covariance_stamped); -} - -tf2::Transform ArTagBasedLocalizer::aruco_marker_to_tf2(const aruco::Marker & marker) -{ - cv::Mat rot(3, 3, CV_64FC1); - cv::Mat r_vec64; - marker.Rvec.convertTo(r_vec64, CV_64FC1); - cv::Rodrigues(r_vec64, rot); - cv::Mat tran64; - marker.Tvec.convertTo(tran64, CV_64FC1); - - tf2::Matrix3x3 tf_rot( - rot.at(0, 0), rot.at(0, 1), rot.at(0, 2), rot.at(1, 0), - rot.at(1, 1), rot.at(1, 2), rot.at(2, 0), rot.at(2, 1), - rot.at(2, 2)); - - tf2::Vector3 tf_orig(tran64.at(0, 0), tran64.at(1, 0), tran64.at(2, 0)); + // for debug + if (image_pub_.getNumSubscribers() > 0) { + cv_bridge::CvImage out_msg; + out_msg.header.stamp = sensor_stamp; + out_msg.encoding = sensor_msgs::image_encodings::RGB8; + out_msg.image = in_image; + image_pub_.publish(out_msg.toImageMsg()); + } - return tf2::Transform(tf_rot, tf_orig); + return landmarks; } diff --git a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp index 889e76eb78ad2..b7dfb45a26ce3 100644 --- a/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp +++ b/localization/landmark_based_localizer/ar_tag_based_localizer/src/ar_tag_based_localizer.hpp @@ -46,11 +46,13 @@ #define AR_TAG_BASED_LOCALIZER_HPP_ #include "landmark_manager/landmark_manager.hpp" +#include "localization_util/smart_pose_buffer.hpp" #include #include #include +#include #include #include @@ -59,6 +61,7 @@ #include #include +#include #include #include #include @@ -72,6 +75,7 @@ class ArTagBasedLocalizer : public rclcpp::Node using Pose = geometry_msgs::msg::Pose; using PoseStamped = geometry_msgs::msg::PoseStamped; using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + using PoseArray = geometry_msgs::msg::PoseArray; using TransformStamped = geometry_msgs::msg::TransformStamped; using MarkerArray = visualization_msgs::msg::MarkerArray; using DiagnosticArray = diagnostic_msgs::msg::DiagnosticArray; @@ -83,23 +87,22 @@ class ArTagBasedLocalizer : public rclcpp::Node private: void map_bin_callback(const HADMapBin::ConstSharedPtr & msg); void image_callback(const Image::ConstSharedPtr & msg); - void cam_info_callback(const CameraInfo & msg); - void ekf_pose_callback(const PoseWithCovarianceStamped & msg); - void publish_pose_as_base_link(const PoseStamped & sensor_to_tag, const std::string & tag_id); - static tf2::Transform aruco_marker_to_tf2(const aruco::Marker & marker); + void cam_info_callback(const CameraInfo::ConstSharedPtr & msg); + void ekf_pose_callback(const PoseWithCovarianceStamped::ConstSharedPtr & msg); + std::vector detect_landmarks(const Image::ConstSharedPtr & msg); // Parameters float marker_size_{}; std::vector target_tag_ids_; std::vector base_covariance_; - double distance_threshold_squared_{}; + double distance_threshold_{}; + bool consider_orientation_{}; double ekf_time_tolerance_{}; double ekf_position_tolerance_{}; // tf std::unique_ptr tf_buffer_; std::unique_ptr tf_listener_; - std::unique_ptr tf_broadcaster_; // image transport std::unique_ptr it_; @@ -112,6 +115,7 @@ class ArTagBasedLocalizer : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Publisher::SharedPtr pose_array_pub_; image_transport::Publisher image_pub_; rclcpp::Publisher::SharedPtr pose_pub_; rclcpp::Publisher::SharedPtr diag_pub_; @@ -120,8 +124,8 @@ class ArTagBasedLocalizer : public rclcpp::Node aruco::MarkerDetector detector_; aruco::CameraParameters cam_param_; bool cam_info_received_; - PoseWithCovarianceStamped latest_ekf_pose_{}; - std::map landmark_map_; + std::unique_ptr ekf_pose_buffer_; + landmark_manager::LandmarkManager landmark_manager_; }; #endif // AR_TAG_BASED_LOCALIZER_HPP_ diff --git a/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg b/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg new file mode 100644 index 0000000000000..4e548d08623ae --- /dev/null +++ b/localization/landmark_based_localizer/doc_image/consider_orientation.drawio.svg @@ -0,0 +1,76 @@ + + + + + + + Self pose + + + + + + relative pose + + + + Detected + Landmark + + + + Mapped + Landmark + + + + New self pose + + + + + + relative pose + + + + consider_orientation = true + + + + + Self pose + + + + Detected + Landmark + + + + Mapped + Landmark + + + + New self pose + + + + consider_orientation = false + + + + + + + diff --git a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp index 9a22ee13f60ae..fba419f746b5e 100644 --- a/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp +++ b/localization/landmark_based_localizer/landmark_manager/include/landmark_manager/landmark_manager.hpp @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -34,12 +35,25 @@ struct Landmark geometry_msgs::msg::Pose pose; }; -std::vector parse_landmarks( - const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, - const std::string & target_subtype, const rclcpp::Logger & logger); - -visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( - const std::vector & landmarks); +class LandmarkManager +{ +public: + void parse_landmarks( + const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, + const std::string & target_subtype, const rclcpp::Logger & logger); + + [[nodiscard]] visualization_msgs::msg::MarkerArray get_landmarks_as_marker_array_msg() const; + + [[nodiscard]] geometry_msgs::msg::Pose calculate_new_self_pose( + const std::vector & detected_landmarks, + const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const; + +private: + // To allow multiple landmarks with the same id to be registered on a vector_map, + // manage vectors by having them in a std::map. + // landmarks_map_[""] = [pose0, pose1, ...] + std::map> landmarks_map_; +}; } // namespace landmark_manager diff --git a/localization/landmark_based_localizer/landmark_manager/package.xml b/localization/landmark_based_localizer/landmark_manager/package.xml index 1a35ae6a338d1..2e0e252386d1f 100644 --- a/localization/landmark_based_localizer/landmark_manager/package.xml +++ b/localization/landmark_based_localizer/landmark_manager/package.xml @@ -18,6 +18,7 @@ autoware_auto_mapping_msgs geometry_msgs lanelet2_extension + localization_util rclcpp tf2_eigen diff --git a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp index 6d1698daf5eae..875f04edd8c47 100644 --- a/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp +++ b/localization/landmark_based_localizer/landmark_manager/src/landmark_manager.cpp @@ -15,6 +15,8 @@ #include "landmark_manager/landmark_manager.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" +#include "localization_util/util_func.hpp" +#include "tier4_autoware_utils/geometry/geometry.hpp" #include #include @@ -25,7 +27,7 @@ namespace landmark_manager { -std::vector parse_landmarks( +void LandmarkManager::parse_landmarks( const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr & msg, const std::string & target_subtype, const rclcpp::Logger & logger) { @@ -36,8 +38,6 @@ std::vector parse_landmarks( lanelet::LaneletMapPtr lanelet_map_ptr{std::make_shared()}; lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_ptr); - std::vector landmarks; - for (const auto & poly : lanelet_map_ptr->polygonLayer) { const std::string type{poly.attributeOr(lanelet::AttributeName::Type, "none")}; if (type != "pose_marker") { @@ -48,8 +48,8 @@ std::vector parse_landmarks( continue; } - // Get marker_id - const std::string marker_id = poly.attributeOr("marker_id", "none"); + // Get landmark_id + const std::string landmark_id = poly.attributeOr("marker_id", "none"); // Get 4 vertices const auto & vertices = poly.basicPolygon(); @@ -58,7 +58,7 @@ std::vector parse_landmarks( continue; } - // 4 vertices represent the marker vertices in counterclockwise order + // 4 vertices represent the landmark vertices in counterclockwise order // Calculate the volume by considering it as a tetrahedron const auto & v0 = vertices[0]; const auto & v1 = vertices[1]; @@ -98,8 +98,8 @@ std::vector parse_landmarks( pose.orientation.w = q.w(); // Add - landmarks.push_back(Landmark{marker_id, pose}); - RCLCPP_INFO_STREAM(logger, "id: " << marker_id); + landmarks_map_[landmark_id].push_back(pose); + RCLCPP_INFO_STREAM(logger, "id: " << landmark_id); RCLCPP_INFO_STREAM( logger, "(x, y, z) = " << pose.position.x << ", " << pose.position.y << ", " << pose.position.z); @@ -107,54 +107,117 @@ std::vector parse_landmarks( logger, "q = " << pose.orientation.x << ", " << pose.orientation.y << ", " << pose.orientation.z << ", " << pose.orientation.w); } - - return landmarks; } -visualization_msgs::msg::MarkerArray convert_landmarks_to_marker_array_msg( - const std::vector & landmarks) +visualization_msgs::msg::MarkerArray LandmarkManager::get_landmarks_as_marker_array_msg() const { int32_t id = 0; visualization_msgs::msg::MarkerArray marker_array; - for (const auto & [id_str, pose] : landmarks) { - // publish cube as a thin board - visualization_msgs::msg::Marker cube_marker; - cube_marker.header.frame_id = "map"; - cube_marker.header.stamp = rclcpp::Clock().now(); - cube_marker.ns = "landmark_cube"; - cube_marker.id = id; - cube_marker.type = visualization_msgs::msg::Marker::CUBE; - cube_marker.action = visualization_msgs::msg::Marker::ADD; - cube_marker.pose = pose; - cube_marker.scale.x = 1.0; - cube_marker.scale.y = 2.0; - cube_marker.scale.z = 0.1; - cube_marker.color.a = 0.5; - cube_marker.color.r = 0.0; - cube_marker.color.g = 1.0; - cube_marker.color.b = 0.0; - marker_array.markers.push_back(cube_marker); - - // publish text - visualization_msgs::msg::Marker text_marker; - text_marker.header.frame_id = "map"; - text_marker.header.stamp = rclcpp::Clock().now(); - text_marker.ns = "landmark_text"; - text_marker.id = id; - text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; - text_marker.action = visualization_msgs::msg::Marker::ADD; - text_marker.pose = pose; - text_marker.text = "(" + id_str + ")"; - text_marker.scale.z = 0.5; - text_marker.color.a = 1.0; - text_marker.color.r = 1.0; - text_marker.color.g = 0.0; - text_marker.color.b = 0.0; - marker_array.markers.push_back(text_marker); - - id++; + for (const auto & [landmark_id_str, landmark_poses] : landmarks_map_) { + for (const auto & pose : landmark_poses) { + // publish cube as a thin board + visualization_msgs::msg::Marker cube_marker; + cube_marker.header.frame_id = "map"; + cube_marker.header.stamp = rclcpp::Clock().now(); + cube_marker.ns = "landmark_cube"; + cube_marker.id = id; + cube_marker.type = visualization_msgs::msg::Marker::CUBE; + cube_marker.action = visualization_msgs::msg::Marker::ADD; + cube_marker.pose = pose; + cube_marker.scale.x = 1.0; + cube_marker.scale.y = 2.0; + cube_marker.scale.z = 0.1; + cube_marker.color.a = 0.5; + cube_marker.color.r = 0.0; + cube_marker.color.g = 1.0; + cube_marker.color.b = 0.0; + marker_array.markers.push_back(cube_marker); + + // publish text + visualization_msgs::msg::Marker text_marker; + text_marker.header.frame_id = "map"; + text_marker.header.stamp = rclcpp::Clock().now(); + text_marker.ns = "landmark_text"; + text_marker.id = id; + text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + text_marker.action = visualization_msgs::msg::Marker::ADD; + text_marker.pose = pose; + text_marker.text = "(" + landmark_id_str + ")"; + text_marker.scale.z = 0.5; + text_marker.color.a = 1.0; + text_marker.color.r = 1.0; + text_marker.color.g = 0.0; + text_marker.color.b = 0.0; + marker_array.markers.push_back(text_marker); + + id++; + } } return marker_array; } +geometry_msgs::msg::Pose LandmarkManager::calculate_new_self_pose( + const std::vector & detected_landmarks, + const geometry_msgs::msg::Pose & self_pose, const bool consider_orientation) const +{ + using Pose = geometry_msgs::msg::Pose; + + Pose min_new_self_pose; + double min_distance = std::numeric_limits::max(); + + for (const landmark_manager::Landmark & landmark : detected_landmarks) { + // Firstly, landmark pose is base_link + const Pose & detected_landmark_on_base_link = landmark.pose; + + // convert base_link to map + const Pose detected_landmark_on_map = + tier4_autoware_utils::transformPose(detected_landmark_on_base_link, self_pose); + + // match to map + if (landmarks_map_.count(landmark.id) == 0) { + continue; + } + + // check all poses + for (const Pose mapped_landmark_on_map : landmarks_map_.at(landmark.id)) { + // check distance + const double curr_distance = tier4_autoware_utils::calcDistance3d( + mapped_landmark_on_map.position, detected_landmark_on_map.position); + if (curr_distance > min_distance) { + continue; + } + + if (consider_orientation) { + const Eigen::Affine3d landmark_pose = pose_to_affine3d(mapped_landmark_on_map); + const Eigen::Affine3d landmark_to_base_link = + pose_to_affine3d(detected_landmark_on_base_link).inverse(); + const Eigen::Affine3d new_self_pose_eigen = landmark_pose * landmark_to_base_link; + + const Pose new_self_pose = matrix4f_to_pose(new_self_pose_eigen.matrix().cast()); + + // update + min_distance = curr_distance; + min_new_self_pose = new_self_pose; + } else { + const double diff_x = + mapped_landmark_on_map.position.x - detected_landmark_on_map.position.x; + const double diff_y = + mapped_landmark_on_map.position.y - detected_landmark_on_map.position.y; + const double diff_z = + mapped_landmark_on_map.position.z - detected_landmark_on_map.position.z; + Pose new_self_pose = self_pose; + new_self_pose.position.x += diff_x; + new_self_pose.position.y += diff_y; + new_self_pose.position.z += diff_z; + + // update + min_distance = curr_distance; + min_new_self_pose = new_self_pose; + } + } + } + + return min_new_self_pose; +} + } // namespace landmark_manager diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index 9490ffb67723b..ade446020d101 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/localization/localization_util/CMakeLists.txt @@ -6,10 +6,18 @@ autoware_package() ament_auto_add_library(localization_util SHARED src/util_func.cpp - src/pose_array_interpolator.cpp src/tf2_listener_module.cpp + src/smart_pose_buffer.cpp ) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + ament_auto_add_gtest(test_smart_pose_buffer + test/test_smart_pose_buffer.cpp + src/smart_pose_buffer.cpp + ) +endif() + ament_auto_package( INSTALL_TO_SHARE ) diff --git a/localization/localization_util/include/localization_util/pose_array_interpolator.hpp b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp similarity index 55% rename from localization/localization_util/include/localization_util/pose_array_interpolator.hpp rename to localization/localization_util/include/localization_util/smart_pose_buffer.hpp index 998d8465733f5..2a2a775a9280c 100644 --- a/localization/localization_util/include/localization_util/pose_array_interpolator.hpp +++ b/localization/localization_util/include/localization_util/smart_pose_buffer.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ -#define LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#ifndef LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ +#define LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ #include "localization_util/util_func.hpp" @@ -23,33 +23,39 @@ #include -class PoseArrayInterpolator +class SmartPoseBuffer { private: using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; public: - PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array, - const double & pose_timeout_sec, const double & pose_distance_tolerance_meters); + struct InterpolateResult + { + PoseWithCovarianceStamped old_pose; + PoseWithCovarianceStamped new_pose; + PoseWithCovarianceStamped interpolated_pose; + }; - PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array); + SmartPoseBuffer() = delete; + SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters); - PoseWithCovarianceStamped get_current_pose(); - PoseWithCovarianceStamped get_old_pose(); - PoseWithCovarianceStamped get_new_pose(); - [[nodiscard]] bool is_success() const; + std::optional interpolate(const rclcpp::Time & target_ros_time); + + void push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr); + + void pop_old(const rclcpp::Time & target_ros_time); + + void clear(); private: rclcpp::Logger logger_; - rclcpp::Clock clock_; - const PoseWithCovarianceStamped::SharedPtr current_pose_ptr_; - PoseWithCovarianceStamped::SharedPtr old_pose_ptr_; - PoseWithCovarianceStamped::SharedPtr new_pose_ptr_; - bool success_; + std::deque pose_buffer_; + std::mutex mutex_; // This mutex is for pose_buffer_ + + const double pose_timeout_sec_; + const double pose_distance_tolerance_meters_; [[nodiscard]] bool validate_time_stamp_difference( const rclcpp::Time & target_time, const rclcpp::Time & reference_time, @@ -59,4 +65,4 @@ class PoseArrayInterpolator const geometry_msgs::msg::Point & reference_point, const double distance_tolerance_m_) const; }; -#endif // LOCALIZATION_UTIL__POSE_ARRAY_INTERPOLATOR_HPP_ +#endif // LOCALIZATION_UTIL__SMART_POSE_BUFFER_HPP_ diff --git a/localization/localization_util/include/localization_util/util_func.hpp b/localization/localization_util/include/localization_util/util_func.hpp index bd9a2b9305e25..0b31333da44d3 100644 --- a/localization/localization_util/include/localization_util/util_func.hpp +++ b/localization/localization_util/include/localization_util/util_func.hpp @@ -46,16 +46,14 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::Pose & pose); geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseStamped & pose); geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad); +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg); + geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b); -void get_nearest_timestamp_pose( - const std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr); - geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp); @@ -64,19 +62,11 @@ geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseWithCovarianceStamped & pose_a, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_b, const rclcpp::Time & time_stamp); -void pop_old_pose( - std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp); - Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose); Eigen::Matrix4f pose_to_matrix4f(const geometry_msgs::msg::Pose & ros_pose); geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_matrix); Eigen::Vector3d point_to_vector3d(const geometry_msgs::msg::Point & ros_pos); -std::vector create_random_pose_array( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num); - template T transform(const T & input, const geometry_msgs::msg::TransformStamped & transform) { @@ -88,7 +78,7 @@ T transform(const T & input, const geometry_msgs::msg::TransformStamped & transf double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); void output_pose_with_cov_to_log( - const rclcpp::Logger logger, const std::string & prefix, + const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov); #endif // LOCALIZATION_UTIL__UTIL_FUNC_HPP_ diff --git a/localization/localization_util/src/pose_array_interpolator.cpp b/localization/localization_util/src/pose_array_interpolator.cpp deleted file mode 100644 index ebc904fcf8d38..0000000000000 --- a/localization/localization_util/src/pose_array_interpolator.cpp +++ /dev/null @@ -1,109 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "localization_util/pose_array_interpolator.hpp" - -PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array) -: logger_(node->get_logger()), - clock_(*node->get_clock()), - current_pose_ptr_(new PoseWithCovarianceStamped), - old_pose_ptr_(new PoseWithCovarianceStamped), - new_pose_ptr_(new PoseWithCovarianceStamped), - success_(true) -{ - get_nearest_timestamp_pose(pose_msg_ptr_array, target_ros_time, old_pose_ptr_, new_pose_ptr_); - const geometry_msgs::msg::PoseStamped interpolated_pose_msg = - interpolate_pose(*old_pose_ptr_, *new_pose_ptr_, target_ros_time); - current_pose_ptr_->header = interpolated_pose_msg.header; - current_pose_ptr_->pose.pose = interpolated_pose_msg.pose; - current_pose_ptr_->pose.covariance = old_pose_ptr_->pose.covariance; -} - -PoseArrayInterpolator::PoseArrayInterpolator( - rclcpp::Node * node, const rclcpp::Time & target_ros_time, - const std::deque & pose_msg_ptr_array, - const double & pose_timeout_sec, const double & pose_distance_tolerance_meters) -: PoseArrayInterpolator(node, target_ros_time, pose_msg_ptr_array) -{ - // check the time stamp - bool is_old_pose_valid = - validate_time_stamp_difference(old_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); - bool is_new_pose_valid = - validate_time_stamp_difference(new_pose_ptr_->header.stamp, target_ros_time, pose_timeout_sec); - - // check the position jumping (ex. immediately after the initial pose estimation) - bool is_pose_diff_valid = validate_position_difference( - old_pose_ptr_->pose.pose.position, new_pose_ptr_->pose.pose.position, - pose_distance_tolerance_meters); - - // all validations must be true - if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { - success_ = false; - RCLCPP_WARN(logger_, "Validation error."); - } -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_current_pose() -{ - return *current_pose_ptr_; -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_old_pose() -{ - return *old_pose_ptr_; -} - -geometry_msgs::msg::PoseWithCovarianceStamped PoseArrayInterpolator::get_new_pose() -{ - return *new_pose_ptr_; -} - -bool PoseArrayInterpolator::is_success() const -{ - return success_; -} - -bool PoseArrayInterpolator::validate_time_stamp_difference( - const rclcpp::Time & target_time, const rclcpp::Time & reference_time, - const double time_tolerance_sec) const -{ - const double dt = std::abs((target_time - reference_time).seconds()); - bool success = dt < time_tolerance_sec; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " - "difference is %lf[sec] (the tolerance is %lf[sec]).", - reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); - } - return success; -} - -bool PoseArrayInterpolator::validate_position_difference( - const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, - const double distance_tolerance_m_) const -{ - double distance = norm(target_point, reference_point); - bool success = distance < distance_tolerance_m_; - if (!success) { - RCLCPP_WARN( - logger_, - "Validation error. The distance from reference position to target position is %lf[m] (the " - "tolerance is %lf[m]).", - distance, distance_tolerance_m_); - } - return success; -} diff --git a/localization/localization_util/src/smart_pose_buffer.cpp b/localization/localization_util/src/smart_pose_buffer.cpp new file mode 100644 index 0000000000000..ba293109dcc4d --- /dev/null +++ b/localization/localization_util/src/smart_pose_buffer.cpp @@ -0,0 +1,148 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_util/smart_pose_buffer.hpp" + +SmartPoseBuffer::SmartPoseBuffer( + const rclcpp::Logger & logger, const double & pose_timeout_sec, + const double & pose_distance_tolerance_meters) +: logger_(logger), + pose_timeout_sec_(pose_timeout_sec), + pose_distance_tolerance_meters_(pose_distance_tolerance_meters) +{ +} + +std::optional SmartPoseBuffer::interpolate( + const rclcpp::Time & target_ros_time) +{ + InterpolateResult result; + + { + std::lock_guard lock(mutex_); + + if (pose_buffer_.size() < 2) { + RCLCPP_INFO(logger_, "pose_buffer_.size() < 2"); + return std::nullopt; + } + + const rclcpp::Time time_first = pose_buffer_.front()->header.stamp; + const rclcpp::Time time_last = pose_buffer_.back()->header.stamp; + + if (target_ros_time < time_first || time_last < target_ros_time) { + return std::nullopt; + } + + // get the nearest poses + result.old_pose = *pose_buffer_.front(); + for (const PoseWithCovarianceStamped::ConstSharedPtr & pose_cov_msg_ptr : pose_buffer_) { + result.new_pose = *pose_cov_msg_ptr; + const rclcpp::Time pose_time_stamp = result.new_pose.header.stamp; + if (pose_time_stamp > target_ros_time) { + break; + } + result.old_pose = *pose_cov_msg_ptr; + } + } + + // check the time stamp + const bool is_old_pose_valid = validate_time_stamp_difference( + result.old_pose.header.stamp, target_ros_time, pose_timeout_sec_); + const bool is_new_pose_valid = validate_time_stamp_difference( + result.new_pose.header.stamp, target_ros_time, pose_timeout_sec_); + + // check the position jumping (ex. immediately after the initial pose estimation) + const bool is_pose_diff_valid = validate_position_difference( + result.old_pose.pose.pose.position, result.new_pose.pose.pose.position, + pose_distance_tolerance_meters_); + + // all validations must be true + if (!(is_old_pose_valid && is_new_pose_valid && is_pose_diff_valid)) { + return std::nullopt; + } + + // interpolate the pose + const geometry_msgs::msg::PoseStamped interpolated_pose_msg = + interpolate_pose(result.old_pose, result.new_pose, target_ros_time); + result.interpolated_pose.header = interpolated_pose_msg.header; + result.interpolated_pose.pose.pose = interpolated_pose_msg.pose; + // This does not interpolate the covariance. + // interpolated_pose.pose.covariance is always set to old_pose.covariance + result.interpolated_pose.pose.covariance = result.old_pose.pose.covariance; + return result; +} + +void SmartPoseBuffer::push_back(const PoseWithCovarianceStamped::ConstSharedPtr & pose_msg_ptr) +{ + std::lock_guard lock(mutex_); + if (!pose_buffer_.empty()) { + // Check for non-chronological timestamp order + // This situation can arise when replaying a rosbag multiple times + const rclcpp::Time end_time = pose_buffer_.back()->header.stamp; + const rclcpp::Time msg_time = pose_msg_ptr->header.stamp; + if (msg_time < end_time) { + // Clear the buffer if timestamps are reversed to maintain chronological order + pose_buffer_.clear(); + } + } + pose_buffer_.push_back(pose_msg_ptr); +} + +void SmartPoseBuffer::pop_old(const rclcpp::Time & target_ros_time) +{ + std::lock_guard lock(mutex_); + while (!pose_buffer_.empty()) { + if (rclcpp::Time(pose_buffer_.front()->header.stamp) >= target_ros_time) { + break; + } + pose_buffer_.pop_front(); + } +} + +void SmartPoseBuffer::clear() +{ + std::lock_guard lock(mutex_); + pose_buffer_.clear(); +} + +bool SmartPoseBuffer::validate_time_stamp_difference( + const rclcpp::Time & target_time, const rclcpp::Time & reference_time, + const double time_tolerance_sec) const +{ + const double dt = std::abs((target_time - reference_time).seconds()); + const bool success = dt < time_tolerance_sec; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The reference time is %lf[sec], but the target time is %lf[sec]. The " + "difference is %lf[sec] (the tolerance is %lf[sec]).", + reference_time.seconds(), target_time.seconds(), dt, time_tolerance_sec); + } + return success; +} + +bool SmartPoseBuffer::validate_position_difference( + const geometry_msgs::msg::Point & target_point, const geometry_msgs::msg::Point & reference_point, + const double distance_tolerance_m_) const +{ + const double distance = norm(target_point, reference_point); + const bool success = distance < distance_tolerance_m_; + if (!success) { + RCLCPP_WARN( + logger_, + "Validation error. The distance from reference position to target position is %lf[m] (the " + "tolerance is %lf[m]).", + distance, distance_tolerance_m_); + } + return success; +} diff --git a/localization/localization_util/src/util_func.cpp b/localization/localization_util/src/util_func.cpp index 865cc02cff293..bb32741067e65 100644 --- a/localization/localization_util/src/util_func.cpp +++ b/localization/localization_util/src/util_func.cpp @@ -16,8 +16,6 @@ #include "localization_util/matrix_type.hpp" -static std::random_device seed_gen; - // ref by http://takacity.blog.fc2.com/blog-entry-69.html std_msgs::msg::ColorRGBA exchange_color_crc(double x) { @@ -82,6 +80,28 @@ geometry_msgs::msg::Vector3 get_rpy(const geometry_msgs::msg::PoseWithCovariance return get_rpy(pose.pose.pose); } +geometry_msgs::msg::Quaternion rpy_rad_to_quaternion( + const double r_rad, const double p_rad, const double y_rad) +{ + tf2::Quaternion q; + q.setRPY(r_rad, p_rad, y_rad); + geometry_msgs::msg::Quaternion quaternion_msg; + quaternion_msg.x = q.x(); + quaternion_msg.y = q.y(); + quaternion_msg.z = q.z(); + quaternion_msg.w = q.w(); + return quaternion_msg; +} + +geometry_msgs::msg::Quaternion rpy_deg_to_quaternion( + const double r_deg, const double p_deg, const double y_deg) +{ + const double r_rad = r_deg * M_PI / 180.0; + const double p_rad = p_deg * M_PI / 180.0; + const double y_rad = y_deg * M_PI / 180.0; + return rpy_rad_to_quaternion(r_rad, p_rad, y_rad); +} + geometry_msgs::msg::Twist calc_twist( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b) { @@ -116,29 +136,6 @@ geometry_msgs::msg::Twist calc_twist( return twist; } -void get_nearest_timestamp_pose( - const std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_old_pose_cov_msg_ptr, - geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr & output_new_pose_cov_msg_ptr) -{ - for (const auto & pose_cov_msg_ptr : pose_cov_msg_ptr_array) { - output_new_pose_cov_msg_ptr = - std::const_pointer_cast(pose_cov_msg_ptr); - const rclcpp::Time pose_time_stamp = output_new_pose_cov_msg_ptr->header.stamp; - if (pose_time_stamp > time_stamp) { - // TODO(Tier IV): refactor - const rclcpp::Time old_pose_time_stamp = output_old_pose_cov_msg_ptr->header.stamp; - if (old_pose_time_stamp.seconds() == 0.0) { - output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; - } - break; - } - output_old_pose_cov_msg_ptr = output_new_pose_cov_msg_ptr; - } -} - geometry_msgs::msg::PoseStamped interpolate_pose( const geometry_msgs::msg::PoseStamped & pose_a, const geometry_msgs::msg::PoseStamped & pose_b, const rclcpp::Time & time_stamp) @@ -193,19 +190,6 @@ geometry_msgs::msg::PoseStamped interpolate_pose( return interpolate_pose(tmp_pose_a, tmp_pose_b, time_stamp); } -void pop_old_pose( - std::deque & - pose_cov_msg_ptr_array, - const rclcpp::Time & time_stamp) -{ - while (!pose_cov_msg_ptr_array.empty()) { - if (rclcpp::Time(pose_cov_msg_ptr_array.front()->header.stamp) >= time_stamp) { - break; - } - pose_cov_msg_ptr_array.pop_front(); - } -} - Eigen::Affine3d pose_to_affine3d(const geometry_msgs::msg::Pose & ros_pose) { Eigen::Affine3d eigen_pose; @@ -237,49 +221,6 @@ geometry_msgs::msg::Pose matrix4f_to_pose(const Eigen::Matrix4f & eigen_pose_mat return ros_pose; } -std::vector create_random_pose_array( - const geometry_msgs::msg::PoseWithCovarianceStamped & base_pose_with_cov, const int particle_num) -{ - std::default_random_engine engine(seed_gen()); - const Eigen::Map covariance = - make_eigen_covariance(base_pose_with_cov.pose.covariance); - - std::normal_distribution<> x_distribution(0.0, std::sqrt(covariance(0, 0))); - std::normal_distribution<> y_distribution(0.0, std::sqrt(covariance(1, 1))); - std::normal_distribution<> z_distribution(0.0, std::sqrt(covariance(2, 2))); - std::normal_distribution<> roll_distribution(0.0, std::sqrt(covariance(3, 3))); - std::normal_distribution<> pitch_distribution(0.0, std::sqrt(covariance(4, 4))); - std::normal_distribution<> yaw_distribution(0.0, std::sqrt(covariance(5, 5))); - - const auto base_rpy = get_rpy(base_pose_with_cov); - - std::vector poses; - for (int i = 0; i < particle_num; ++i) { - geometry_msgs::msg::Vector3 xyz; - geometry_msgs::msg::Vector3 rpy; - - xyz.x = base_pose_with_cov.pose.pose.position.x + x_distribution(engine); - xyz.y = base_pose_with_cov.pose.pose.position.y + y_distribution(engine); - xyz.z = base_pose_with_cov.pose.pose.position.z + z_distribution(engine); - rpy.x = base_rpy.x + roll_distribution(engine); - rpy.y = base_rpy.y + pitch_distribution(engine); - rpy.z = base_rpy.z + yaw_distribution(engine); - - tf2::Quaternion tf_quaternion; - tf_quaternion.setRPY(rpy.x, rpy.y, rpy.z); - - geometry_msgs::msg::Pose pose; - pose.position.x = xyz.x; - pose.position.y = xyz.y; - pose.position.z = xyz.z; - pose.orientation = tf2::toMsg(tf_quaternion); - - poses.push_back(pose); - } - - return poses; -} - double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) { const double dx = p1.x - p2.x; @@ -289,7 +230,7 @@ double norm(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Poin } void output_pose_with_cov_to_log( - const rclcpp::Logger logger, const std::string & prefix, + const rclcpp::Logger & logger, const std::string & prefix, const geometry_msgs::msg::PoseWithCovarianceStamped & pose_with_cov) { const Eigen::Map covariance = diff --git a/localization/localization_util/test/test_smart_pose_buffer.cpp b/localization/localization_util/test/test_smart_pose_buffer.cpp new file mode 100644 index 0000000000000..2520e458f2d33 --- /dev/null +++ b/localization/localization_util/test/test_smart_pose_buffer.cpp @@ -0,0 +1,114 @@ +// Copyright 2023- Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "localization_util/smart_pose_buffer.hpp" +#include "localization_util/util_func.hpp" + +#include +#include + +#include +#include + +#include +#include +#include + +using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + +bool compare_pose( + const PoseWithCovarianceStamped & pose_a, const PoseWithCovarianceStamped & pose_b) +{ + return pose_a.header.stamp == pose_b.header.stamp && + pose_a.header.frame_id == pose_b.header.frame_id && + pose_a.pose.covariance == pose_b.pose.covariance && + pose_a.pose.pose.position.x == pose_b.pose.pose.position.x && + pose_a.pose.pose.position.y == pose_b.pose.pose.position.y && + pose_a.pose.pose.position.z == pose_b.pose.pose.position.z && + pose_a.pose.pose.orientation.x == pose_b.pose.pose.orientation.x && + pose_a.pose.pose.orientation.y == pose_b.pose.pose.orientation.y && + pose_a.pose.pose.orientation.z == pose_b.pose.pose.orientation.z && + pose_a.pose.pose.orientation.w == pose_b.pose.pose.orientation.w; +} + +class TestSmartPoseBuffer : public ::testing::Test +{ +protected: + void SetUp() override {} + + void TearDown() override {} +}; + +TEST_F(TestSmartPoseBuffer, interpolate_pose) // NOLINT +{ + rclcpp::Logger logger = rclcpp::get_logger("test_logger"); + const double pose_timeout_sec = 10.0; + const double pose_distance_tolerance_meters = 100.0; + SmartPoseBuffer smart_pose_buffer(logger, pose_timeout_sec, pose_distance_tolerance_meters); + + // first data + PoseWithCovarianceStamped::SharedPtr old_pose_ptr = std::make_shared(); + old_pose_ptr->header.stamp.sec = 10; + old_pose_ptr->header.stamp.nanosec = 0; + old_pose_ptr->pose.pose.position.x = 10.0; + old_pose_ptr->pose.pose.position.y = 20.0; + old_pose_ptr->pose.pose.position.z = 0.0; + old_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 0.0); + smart_pose_buffer.push_back(old_pose_ptr); + + // second data + PoseWithCovarianceStamped::SharedPtr new_pose_ptr = std::make_shared(); + new_pose_ptr->header.stamp.sec = 20; + new_pose_ptr->header.stamp.nanosec = 0; + new_pose_ptr->pose.pose.position.x = 20.0; + new_pose_ptr->pose.pose.position.y = 40.0; + new_pose_ptr->pose.pose.position.z = 0.0; + new_pose_ptr->pose.pose.orientation = rpy_deg_to_quaternion(0.0, 0.0, 90.0); + smart_pose_buffer.push_back(new_pose_ptr); + + // interpolate + builtin_interfaces::msg::Time target_ros_time_msg; + target_ros_time_msg.sec = 15; + target_ros_time_msg.nanosec = 0; + const std::optional & interpolate_result = + smart_pose_buffer.interpolate(target_ros_time_msg); + ASSERT_TRUE(interpolate_result.has_value()); + const SmartPoseBuffer::InterpolateResult result = interpolate_result.value(); + + // check old + EXPECT_TRUE(compare_pose(result.old_pose, *old_pose_ptr)); + + // check new + EXPECT_TRUE(compare_pose(result.new_pose, *new_pose_ptr)); + + // check interpolated + EXPECT_EQ(result.interpolated_pose.header.stamp.sec, 15); + EXPECT_EQ(result.interpolated_pose.header.stamp.nanosec, static_cast(0)); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.x, 15.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.y, 30.0); + EXPECT_EQ(result.interpolated_pose.pose.pose.position.z, 0.0); + const auto rpy = get_rpy(result.interpolated_pose.pose.pose); + EXPECT_NEAR(rpy.x * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.y * 180 / M_PI, 0.0, 1e-6); + EXPECT_NEAR(rpy.z * 180 / M_PI, 45.0, 1e-6); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/localization/ndt_scan_matcher/CMakeLists.txt b/localization/ndt_scan_matcher/CMakeLists.txt index 4beecc2625fe3..2ad4a47b833d1 100644 --- a/localization/ndt_scan_matcher/CMakeLists.txt +++ b/localization/ndt_scan_matcher/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(PCL REQUIRED COMPONENTS common io registration) include_directories(${PCL_INCLUDE_DIRS}) ament_auto_add_executable(ndt_scan_matcher - src/debug.cpp + src/particle.cpp src/ndt_scan_matcher_node.cpp src/ndt_scan_matcher_core.cpp src/map_module.cpp diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp deleted file mode 100644 index ccc3b59e63328..0000000000000 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/debug.hpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2015-2019 Autoware Foundation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef NDT_SCAN_MATCHER__DEBUG_HPP_ -#define NDT_SCAN_MATCHER__DEBUG_HPP_ - -#include "ndt_scan_matcher/particle.hpp" - -#include - -#include - -visualization_msgs::msg::MarkerArray make_debug_markers( - const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, - const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i); - -#endif // NDT_SCAN_MATCHER__DEBUG_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp index 78052cb8198a3..da0ae27a7808e 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/map_update_module.hpp @@ -17,7 +17,6 @@ #include "localization_util/tf2_listener_module.hpp" #include "localization_util/util_func.hpp" -#include "ndt_scan_matcher/debug.hpp" #include "ndt_scan_matcher/particle.hpp" #include diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp index 6f9420f5bc71a..7f4a17a65a7e1 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/ndt_scan_matcher_core.hpp @@ -17,6 +17,7 @@ #define FMT_HEADER_ONLY +#include "localization_util/smart_pose_buffer.hpp" #include "localization_util/tf2_listener_module.hpp" #include "ndt_scan_matcher/map_module.hpp" #include "ndt_scan_matcher/map_update_module.hpp" @@ -128,8 +129,6 @@ class NDTScanMatcher : public rclcpp::Node const pclomp::NdtResult & ndt_result, const Eigen::Matrix4f & initial_pose_matrix, const rclcpp::Time & sensor_ros_time); - std::optional interpolate_regularization_pose( - const rclcpp::Time & sensor_ros_time); void add_regularization_pose(const rclcpp::Time & sensor_ros_time); void publish_diagnostic(); @@ -187,8 +186,8 @@ class NDTScanMatcher : public rclcpp::Node double converged_param_transform_probability_; double converged_param_nearest_voxel_transformation_likelihood_; - int initial_estimate_particles_num_; - int n_startup_trials_; + int64_t initial_estimate_particles_num_; + int64_t n_startup_trials_; double lidar_topic_timeout_sec_; double initial_pose_timeout_sec_; double initial_pose_distance_tolerance_m_; @@ -198,16 +197,12 @@ class NDTScanMatcher : public rclcpp::Node std::vector initial_pose_offset_model_; std::array output_pose_covariance_; - std::deque - initial_pose_msg_ptr_array_; std::mutex ndt_ptr_mtx_; - std::mutex initial_pose_array_mtx_; + std::unique_ptr initial_pose_buffer_; // variables for regularization const bool regularization_enabled_; // whether to use longitudinal regularization - std::deque - regularization_pose_msg_ptr_array_; // queue for storing regularization base poses - std::mutex regularization_mutex_; // mutex for regularization_pose_msg_ptr_array_ + std::unique_ptr regularization_pose_buffer_; bool is_activated_; std::shared_ptr tf2_listener_module_; @@ -222,7 +217,7 @@ class NDTScanMatcher : public rclcpp::Node bool use_dynamic_map_loading_; // The execution time which means probably NDT cannot matches scans properly - int critical_upper_bound_exe_time_ms_; + int64_t critical_upper_bound_exe_time_ms_; }; #endif // NDT_SCAN_MATCHER__NDT_SCAN_MATCHER_CORE_HPP_ diff --git a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp index 740acf335099c..f1c05bfe98551 100644 --- a/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp +++ b/localization/ndt_scan_matcher/include/ndt_scan_matcher/particle.hpp @@ -16,6 +16,9 @@ #define NDT_SCAN_MATCHER__PARTICLE_HPP_ #include +#include + +#include struct Particle { @@ -31,4 +34,8 @@ struct Particle int iteration; }; +void push_debug_markers( + visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, + const std::string & map_frame_, const Particle & particle, const size_t i); + #endif // NDT_SCAN_MATCHER__PARTICLE_HPP_ diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index 1317b8bacf47f..783df34e6c0f8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp @@ -15,7 +15,6 @@ #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp" #include "localization_util/matrix_type.hpp" -#include "localization_util/pose_array_interpolator.hpp" #include "localization_util/util_func.hpp" #include "ndt_scan_matcher/particle.hpp" #include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp" @@ -53,19 +52,6 @@ tier4_debug_msgs::msg::Int32Stamped make_int32_stamped( return tier4_debug_msgs::build().stamp(stamp).data(data); } -std::vector create_initial_pose_offset_model( - const std::vector & x, const std::vector & y) -{ - int size = x.size(); - std::vector initial_pose_offset_model(size); - for (int i = 0; i < size; i++) { - initial_pose_offset_model[i].x() = x[i]; - initial_pose_offset_model[i].y() = y[i]; - } - - return initial_pose_offset_model; -} - Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( const Eigen::Matrix2d & matrix) { @@ -74,9 +60,8 @@ Eigen::Matrix2d find_rotation_matrix_aligning_covariance_to_principal_axes( const Eigen::Vector2d eigen_vec = eigensolver.eigenvectors().col(0); const double th = std::atan2(eigen_vec.y(), eigen_vec.x()); return Eigen::Rotation2Dd(th).toRotationMatrix(); - } else { - throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } + throw std::runtime_error("Eigen solver failed. Return output_pose_covariance value."); } bool validate_local_optimal_solution_oscillation( @@ -115,14 +100,15 @@ NDTScanMatcher::NDTScanMatcher() state_ptr_(new std::map), inversion_vector_threshold_(-0.9), // Not necessary to extract to ndt_scan_matcher.param.yaml oscillation_threshold_(10), // Not necessary to extract to ndt_scan_matcher.param.yaml + output_pose_covariance_({}), regularization_enabled_(declare_parameter("regularization_enabled")) { (*state_ptr_)["state"] = "Initializing"; is_activated_ = false; - int points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); - points_queue_size = std::max(points_queue_size, 0); - RCLCPP_INFO(get_logger(), "points_queue_size: %d", points_queue_size); + int64_t points_queue_size = this->declare_parameter("input_sensor_points_queue_size"); + points_queue_size = std::max(points_queue_size, (int64_t)0); + RCLCPP_INFO(get_logger(), "points_queue_size: %ld", points_queue_size); base_frame_ = this->declare_parameter("base_frame"); RCLCPP_INFO(get_logger(), "base_frame_id: %s", base_frame_.c_str()); @@ -137,8 +123,8 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon = this->declare_parameter("trans_epsilon"); ndt_params.step_size = this->declare_parameter("step_size"); ndt_params.resolution = this->declare_parameter("resolution"); - ndt_params.max_iterations = this->declare_parameter("max_iterations"); - ndt_params.num_threads = this->declare_parameter("num_threads"); + ndt_params.max_iterations = static_cast(this->declare_parameter("max_iterations")); + ndt_params.num_threads = static_cast(this->declare_parameter("num_threads")); ndt_params.num_threads = std::max(ndt_params.num_threads, 1); ndt_params.regularization_scale_factor = static_cast(this->declare_parameter("regularization_scale_factor")); @@ -149,7 +135,7 @@ NDTScanMatcher::NDTScanMatcher() ndt_params.trans_epsilon, ndt_params.step_size, ndt_params.resolution, ndt_params.max_iterations); - int converged_param_type_tmp = this->declare_parameter("converged_param_type"); + const int64_t converged_param_type_tmp = this->declare_parameter("converged_param_type"); converged_param_type_ = static_cast(converged_param_type_tmp); converged_param_transform_probability_ = @@ -160,13 +146,16 @@ NDTScanMatcher::NDTScanMatcher() lidar_topic_timeout_sec_ = this->declare_parameter("lidar_topic_timeout_sec"); critical_upper_bound_exe_time_ms_ = - this->declare_parameter("critical_upper_bound_exe_time_ms"); + this->declare_parameter("critical_upper_bound_exe_time_ms"); initial_pose_timeout_sec_ = this->declare_parameter("initial_pose_timeout_sec"); initial_pose_distance_tolerance_m_ = this->declare_parameter("initial_pose_distance_tolerance_m"); + initial_pose_buffer_ = std::make_unique( + this->get_logger(), initial_pose_timeout_sec_, initial_pose_distance_tolerance_m_); + use_cov_estimation_ = this->declare_parameter("use_covariance_estimation"); if (use_cov_estimation_) { std::vector initial_pose_offset_model_x = @@ -175,8 +164,12 @@ NDTScanMatcher::NDTScanMatcher() this->declare_parameter>("initial_pose_offset_model_y"); if (initial_pose_offset_model_x.size() == initial_pose_offset_model_y.size()) { - initial_pose_offset_model_ = - create_initial_pose_offset_model(initial_pose_offset_model_x, initial_pose_offset_model_y); + const size_t size = initial_pose_offset_model_x.size(); + initial_pose_offset_model_.resize(size); + for (size_t i = 0; i < size; i++) { + initial_pose_offset_model_[i].x() = initial_pose_offset_model_x[i]; + initial_pose_offset_model_[i].y() = initial_pose_offset_model_y[i]; + } } else { RCLCPP_WARN( get_logger(), @@ -191,8 +184,9 @@ NDTScanMatcher::NDTScanMatcher() output_pose_covariance_[i] = output_pose_covariance[i]; } - initial_estimate_particles_num_ = this->declare_parameter("initial_estimate_particles_num"); - n_startup_trials_ = this->declare_parameter("n_startup_trials"); + initial_estimate_particles_num_ = + this->declare_parameter("initial_estimate_particles_num"); + n_startup_trials_ = this->declare_parameter("n_startup_trials"); estimate_scores_for_degrounded_scan_ = this->declare_parameter("estimate_scores_for_degrounded_scan"); @@ -232,6 +226,9 @@ NDTScanMatcher::NDTScanMatcher() "regularization_pose_with_covariance", 10, std::bind(&NDTScanMatcher::callback_regularization_pose, this, std::placeholders::_1), initial_pose_sub_opt); + const double value_as_unlimited = 1000.0; + regularization_pose_buffer_ = + std::make_unique(this->get_logger(), value_as_unlimited, value_as_unlimited); } sensor_aligned_pose_pub_ = @@ -350,7 +347,8 @@ void NDTScanMatcher::publish_diagnostic() } if ( state_ptr_->count("execution_time") && - std::stod((*state_ptr_)["execution_time"]) >= critical_upper_bound_exe_time_ms_) { + std::stod((*state_ptr_)["execution_time"]) >= + static_cast(critical_upper_bound_exe_time_ms_)) { diag_status_msg.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; diag_status_msg.message += "NDT exe time is too long. (took " + (*state_ptr_)["execution_time"] + " [ms])"; @@ -375,42 +373,21 @@ void NDTScanMatcher::callback_initial_pose( { if (!is_activated_) return; - // lock mutex for initial pose - std::lock_guard initial_pose_array_lock(initial_pose_array_mtx_); - // if rosbag restart, clear buffer - if (!initial_pose_msg_ptr_array_.empty()) { - const builtin_interfaces::msg::Time & t_front = - initial_pose_msg_ptr_array_.front()->header.stamp; - const builtin_interfaces::msg::Time & t_msg = initial_pose_msg_ptr->header.stamp; - if (t_front.sec > t_msg.sec || (t_front.sec == t_msg.sec && t_front.nanosec > t_msg.nanosec)) { - initial_pose_msg_ptr_array_.clear(); - } - } - if (initial_pose_msg_ptr->header.frame_id == map_frame_) { - initial_pose_msg_ptr_array_.push_back(initial_pose_msg_ptr); + initial_pose_buffer_->push_back(initial_pose_msg_ptr); } else { - // get TF from pose_frame to map_frame - auto tf_pose_to_map_ptr = std::make_shared(); - tf2_listener_module_->get_transform( - this->now(), map_frame_, initial_pose_msg_ptr->header.frame_id, tf_pose_to_map_ptr); - - // transform pose_frame to map_frame - auto initial_pose_msg_in_map_ptr = - std::make_shared(); - *initial_pose_msg_in_map_ptr = transform(*initial_pose_msg_ptr, *tf_pose_to_map_ptr); - initial_pose_msg_in_map_ptr->header.stamp = initial_pose_msg_ptr->header.stamp; - initial_pose_msg_ptr_array_.push_back(initial_pose_msg_in_map_ptr); + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *this->get_clock(), 1000, + "Received initial pose message with frame_id " + << initial_pose_msg_ptr->header.frame_id << ", but expected " << map_frame_ + << ". Please check the frame_id in the input topic and ensure it is correct."); } } void NDTScanMatcher::callback_regularization_pose( geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose_conv_msg_ptr) { - // Get lock for regularization_pose_msg_ptr_array_ - std::lock_guard lock(regularization_mutex_); - regularization_pose_msg_ptr_array_.push_back(pose_conv_msg_ptr); - // Release lock for regularization_pose_msg_ptr_array_ + regularization_pose_buffer_->push_back(pose_conv_msg_ptr); } void NDTScanMatcher::callback_sensor_points( @@ -459,17 +436,15 @@ void NDTScanMatcher::callback_sensor_points( if (!is_activated_) return; // calculate initial pose - std::unique_lock initial_pose_array_lock(initial_pose_array_mtx_); - if (initial_pose_msg_ptr_array_.size() <= 1) { - RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No Pose!"); + std::optional interpolation_result_opt = + initial_pose_buffer_->interpolate(sensor_ros_time); + if (!interpolation_result_opt) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1, "No interpolated pose!"); return; } - PoseArrayInterpolator interpolator( - this, sensor_ros_time, initial_pose_msg_ptr_array_, initial_pose_timeout_sec_, - initial_pose_distance_tolerance_m_); - if (!interpolator.is_success()) return; - pop_old_pose(initial_pose_msg_ptr_array_, sensor_ros_time); - initial_pose_array_lock.unlock(); + initial_pose_buffer_->pop_old(sensor_ros_time); + const SmartPoseBuffer::InterpolateResult & interpolation_result = + interpolation_result_opt.value(); // if regularization is enabled and available, set pose to NDT for regularization if (regularization_enabled_) { @@ -483,7 +458,7 @@ void NDTScanMatcher::callback_sensor_points( // perform ndt scan matching const Eigen::Matrix4f initial_pose_matrix = - pose_to_matrix4f(interpolator.get_current_pose().pose.pose); + pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); auto output_cloud = std::make_shared>(); ndt_ptr_->align(*output_cloud, initial_pose_matrix); const pclomp::NdtResult ndt_result = ndt_ptr_->getResult(); @@ -528,7 +503,7 @@ void NDTScanMatcher::callback_sensor_points( const auto exe_time = static_cast(duration_micro_sec) / 1000.0f; // publish - initial_pose_with_covariance_pub_->publish(interpolator.get_current_pose()); + initial_pose_with_covariance_pub_->publish(interpolation_result.interpolated_pose); exe_time_pub_->publish(make_float32_stamped(sensor_ros_time, exe_time)); transform_probability_pub_->publish( make_float32_stamped(sensor_ros_time, ndt_result.transform_probability)); @@ -539,8 +514,8 @@ void NDTScanMatcher::callback_sensor_points( publish_pose(sensor_ros_time, result_pose_msg, ndt_covariance, is_converged); publish_marker(sensor_ros_time, transformation_msg_array); publish_initial_to_result( - sensor_ros_time, result_pose_msg, interpolator.get_current_pose(), interpolator.get_old_pose(), - interpolator.get_new_pose()); + sensor_ros_time, result_pose_msg, interpolation_result.interpolated_pose, + interpolation_result.old_pose, interpolation_result.new_pose); pcl::shared_ptr> sensor_points_in_map_ptr( new pcl::PointCloud); @@ -765,12 +740,12 @@ std::array NDTScanMatcher::estimate_covariance( rot = find_rotation_matrix_aligning_covariance_to_principal_axes( ndt_result.hessian.inverse().block(0, 0, 2, 2)); } catch (const std::exception & e) { - RCLCPP_WARN(get_logger(), e.what()); + RCLCPP_WARN(get_logger(), "Error in Eigen solver: %s", e.what()); return output_pose_covariance_; } // first result is added to mean - const int n = initial_pose_offset_model_.size() + 1; + const int n = static_cast(initial_pose_offset_model_.size()) + 1; const Eigen::Vector2d ndt_pose_2d(ndt_result.pose(0, 3), ndt_result.pose(1, 3)); Eigen::Vector2d mean = ndt_pose_2d; std::vector ndt_pose_2d_vec; @@ -828,42 +803,20 @@ std::array NDTScanMatcher::estimate_covariance( return ndt_covariance; } -std::optional NDTScanMatcher::interpolate_regularization_pose( - const rclcpp::Time & sensor_ros_time) -{ - std::shared_ptr interpolator = nullptr; - { - // Get lock for regularization_pose_msg_ptr_array_ - std::lock_guard lock(regularization_mutex_); - - if (regularization_pose_msg_ptr_array_.empty()) { - return std::nullopt; - } - - interpolator = std::make_shared( - this, sensor_ros_time, regularization_pose_msg_ptr_array_); - - // Remove old poses to make next interpolation more efficient - pop_old_pose(regularization_pose_msg_ptr_array_, sensor_ros_time); - - // Release lock for regularization_pose_msg_ptr_array_ - } - - if (!interpolator || !interpolator->is_success()) { - return std::nullopt; - } - - return pose_to_matrix4f(interpolator->get_current_pose().pose.pose); -} - void NDTScanMatcher::add_regularization_pose(const rclcpp::Time & sensor_ros_time) { ndt_ptr_->unsetRegularizationPose(); - std::optional pose_opt = interpolate_regularization_pose(sensor_ros_time); - if (pose_opt.has_value()) { - ndt_ptr_->setRegularizationPose(pose_opt.value()); - RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); + std::optional interpolation_result_opt = + regularization_pose_buffer_->interpolate(sensor_ros_time); + if (!interpolation_result_opt) { + return; } + regularization_pose_buffer_->pop_old(sensor_ros_time); + const SmartPoseBuffer::InterpolateResult & interpolation_result = + interpolation_result_opt.value(); + const Eigen::Matrix4f pose = pose_to_matrix4f(interpolation_result.interpolated_pose.pose.pose); + ndt_ptr_->setRegularizationPose(pose); + RCLCPP_DEBUG_STREAM(get_logger(), "Regularization pose is set to NDT"); } void NDTScanMatcher::service_trigger_node( @@ -872,8 +825,7 @@ void NDTScanMatcher::service_trigger_node( { is_activated_ = req->data; if (is_activated_) { - std::lock_guard initial_pose_array_lock(initial_pose_array_mtx_); - initial_pose_msg_ptr_array_.clear(); + initial_pose_buffer_->clear(); } res->success = true; } @@ -936,16 +888,16 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( // The range taken by 2 * phi(x) - 1 is [-1, 1], so it can be used as a uniform distribution in // TPE. Let u = 2 * phi(x) - 1, then x = sqrt(2) * erf_inv(u). Computationally, it is not a good // to give erf_inv -1 and 1, so it is rounded off at (-1 + eps, 1 - eps). - const double SQRT2 = std::sqrt(2); - auto uniform_to_normal = [&SQRT2](const double uniform) { + const double sqrt2 = std::sqrt(2); + auto uniform_to_normal = [&sqrt2](const double uniform) { assert(-1.0 <= uniform && uniform <= 1.0); constexpr double epsilon = 1.0e-6; const double clamped = std::clamp(uniform, -1.0 + epsilon, 1.0 - epsilon); - return boost::math::erf_inv(clamped) * SQRT2; + return boost::math::erf_inv(clamped) * sqrt2; }; - auto normal_to_uniform = [&SQRT2](const double normal) { - return boost::math::erf(normal / SQRT2); + auto normal_to_uniform = [&sqrt2](const double normal) { + return boost::math::erf(normal / sqrt2); }; // Optimizing (x, y, z, roll, pitch, yaw) 6 dimensions. @@ -960,7 +912,12 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( std::vector particle_array; auto output_cloud = std::make_shared>(); - for (int i = 0; i < initial_estimate_particles_num_; i++) { + // publish the estimated poses in 20 times to see the progress and to avoid dropping data + visualization_msgs::msg::MarkerArray marker_array; + constexpr int64_t publish_num = 20; + const int64_t publish_interval = initial_estimate_particles_num_ / publish_num; + + for (int64_t i = 0; i < initial_estimate_particles_num_; i++) { const TreeStructuredParzenEstimator::Input input = tpe.get_next_input(); geometry_msgs::msg::Pose initial_pose; @@ -986,10 +943,11 @@ geometry_msgs::msg::PoseWithCovarianceStamped NDTScanMatcher::align_pose( initial_pose, matrix4f_to_pose(ndt_result.pose), ndt_result.transform_probability, ndt_result.iteration_num); particle_array.push_back(particle); - const auto marker_array = make_debug_markers( - get_clock()->now(), map_frame_, tier4_autoware_utils::createMarkerScale(0.3, 0.1, 0.1), - particle, i); - ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + push_debug_markers(marker_array, get_clock()->now(), map_frame_, particle, i); + if ((i + 1) % publish_interval == 0 || (i + 1) == initial_estimate_particles_num_) { + ndt_monte_carlo_initial_pose_marker_pub_->publish(marker_array); + marker_array.markers.clear(); + } const geometry_msgs::msg::Pose pose = matrix4f_to_pose(ndt_result.pose); const geometry_msgs::msg::Vector3 rpy = get_rpy(pose); diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp index d5ea544d3c5e5..6e2ed022f531e 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp +++ b/localization/ndt_scan_matcher/src/ndt_scan_matcher_node.cpp @@ -20,7 +20,7 @@ int main(int argc, char ** argv) { - google::InitGoogleLogging(argv[0]); + google::InitGoogleLogging(argv[0]); // NOLINT google::InstallFailureSignalHandler(); rclcpp::init(argc, argv); diff --git a/localization/ndt_scan_matcher/src/debug.cpp b/localization/ndt_scan_matcher/src/particle.cpp similarity index 83% rename from localization/ndt_scan_matcher/src/debug.cpp rename to localization/ndt_scan_matcher/src/particle.cpp index 941f682de5803..b30703c2761e8 100644 --- a/localization/ndt_scan_matcher/src/debug.cpp +++ b/localization/ndt_scan_matcher/src/particle.cpp @@ -12,24 +12,22 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "ndt_scan_matcher/debug.hpp" +#include "ndt_scan_matcher/particle.hpp" #include "localization_util/util_func.hpp" -visualization_msgs::msg::MarkerArray make_debug_markers( - const builtin_interfaces::msg::Time & stamp, const std::string & map_frame_, - const geometry_msgs::msg::Vector3 & scale, const Particle & particle, const size_t i) +void push_debug_markers( + visualization_msgs::msg::MarkerArray & marker_array, const builtin_interfaces::msg::Time & stamp, + const std::string & map_frame_, const Particle & particle, const size_t i) { - // TODO(Tier IV): getNumSubscribers - // TODO(Tier IV): clear old object - visualization_msgs::msg::MarkerArray marker_array; - visualization_msgs::msg::Marker marker; marker.header.stamp = stamp; marker.header.frame_id = map_frame_; marker.type = visualization_msgs::msg::Marker::ARROW; marker.action = visualization_msgs::msg::Marker::ADD; - marker.scale = scale; + marker.scale.x = 0.3; + marker.scale.y = 0.1; + marker.scale.z = 0.1; marker.id = static_cast(i); marker.lifetime = rclcpp::Duration::from_seconds(10.0); // 10.0 is the lifetime in seconds. @@ -62,6 +60,4 @@ visualization_msgs::msg::MarkerArray make_debug_markers( marker.pose = particle.result_pose; marker.color = exchange_color_crc(static_cast(i) / 100.0); marker_array.markers.push_back(marker); - - return marker_array; } diff --git a/localization/stop_filter/README.md b/localization/stop_filter/README.md index 714824b430d74..261b8c9da392c 100644 --- a/localization/stop_filter/README.md +++ b/localization/stop_filter/README.md @@ -25,7 +25,4 @@ This node aims to: ## Parameters -| Name | Type | Description | -| -------------- | ------ | --------------------------------------------------------------------------------------------- | -| `vx_threshold` | double | longitudinal velocity threshold to determine if the vehicle is stopping [m/s] (default: 0.01) | -| `wz_threshold` | double | yaw velocity threshold to determine if the vehicle is stopping [rad/s] (default: 0.01) | +{{ json_to_markdown("localization/stop_filter/schema/stop_filter.schema.json") }} diff --git a/localization/stop_filter/schema/stop_filter.schema.json b/localization/stop_filter/schema/stop_filter.schema.json new file mode 100644 index 0000000000000..ce6d4b8a2bb72 --- /dev/null +++ b/localization/stop_filter/schema/stop_filter.schema.json @@ -0,0 +1,40 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Stop Filter Node", + "type": "object", + "definitions": { + "stop_filter": { + "type": "object", + "properties": { + "vx_threshold": { + "type": "number", + "description": "Longitudinal velocity threshold to determine if the vehicle is stopping. [m/s]", + "default": "0.01", + "minimum": 0.0 + }, + "wz_threshold": { + "type": "number", + "description": "Yaw velocity threshold to determine if the vehicle is stopping. [rad/s]", + "default": "0.01", + "minimum": 0.0 + } + }, + "required": ["vx_threshold", "wz_threshold"], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/stop_filter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/perception/bytetrack/CMakeLists.txt b/perception/bytetrack/CMakeLists.txt index e481464455a55..c020843af56b5 100644 --- a/perception/bytetrack/CMakeLists.txt +++ b/perception/bytetrack/CMakeLists.txt @@ -29,7 +29,10 @@ target_include_directories(bytetrack_lib $ $ ) -target_link_libraries(bytetrack_lib Eigen3::Eigen) +target_link_libraries(bytetrack_lib + Eigen3::Eigen + yaml-cpp +) # # ROS node @@ -91,4 +94,5 @@ rclcpp_components_register_node(${PROJECT_NAME}_visualizer ament_auto_package(INSTALL_TO_SHARE launch + config ) diff --git a/perception/bytetrack/README.md b/perception/bytetrack/README.md index e58f4bb5892aa..cabbdd2f05836 100644 --- a/perception/bytetrack/README.md +++ b/perception/bytetrack/README.md @@ -6,7 +6,7 @@ The core algorithm, named `ByteTrack`, mainly aims to perform multi-object track Because the algorithm associates almost every detection box including ones with low detection scores, the number of false negatives is expected to decrease by using it. -[demo video](https://user-images.githubusercontent.com/3022416/225920856-745a3bb7-6b35-403d-87b0-6e5085952d70.mp4) +[demo video](https://github.com/YoshiRi/autoware.universe/assets/3022416/40f4c158-657e-48e1-81c2-8ac39152892d) ## Inner-workings / Algorithms @@ -20,6 +20,16 @@ the number of false negatives is expected to decrease by using it. - This package is ported version toward Autoware from [this repository](https://github.com/ifzhang/ByteTrack/tree/main/deploy/TensorRT/cpp) (The C++ implementation by the ByteTrack's authors) +### 2d tracking modification from original codes + +The paper just says that the 2d tracking algorithm is a simple Kalman filter. +Original codes use the `top-left-corner` and `aspect ratio` and `size` as the state vector. + +This is sometimes unstable because the aspectratio can be changed by the occlusion. +So, we use the `top-left` and `size` as the state vector. + +Kalman filter settings can be controlled by the parameters in `config/bytetrack_node.param.yaml`. + ## Inputs / Outputs ### bytetrack_node diff --git a/perception/bytetrack/config/bytetracker.param.yaml b/perception/bytetrack/config/bytetracker.param.yaml new file mode 100644 index 0000000000000..7078f190c87e6 --- /dev/null +++ b/perception/bytetrack/config/bytetracker.param.yaml @@ -0,0 +1,14 @@ +# Kalman filter parameters for 2d tracking +dt: 0.1 # time step[s] +dim_x: 8 # tlbr + velocity +dim_z: 4 # tlbr +q_cov_x: 0.1 +q_cov_y: 0.1 +q_cov_vx: 0.1 +q_cov_vy: 0.1 +r_cov_x: 0.1 +r_cov_y: 0.1 +p0_cov_x: 100.0 +p0_cov_y: 100.0 +p0_cov_vx: 100.0 +p0_cov_vy: 100.0 diff --git a/perception/bytetrack/lib/include/byte_tracker.h b/perception/bytetrack/lib/include/byte_tracker.h index 08394e6b0ccdf..60328c5acfd62 100644 --- a/perception/bytetrack/lib/include/byte_tracker.h +++ b/perception/bytetrack/lib/include/byte_tracker.h @@ -96,5 +96,5 @@ class ByteTracker std::vector tracked_stracks; std::vector lost_stracks; std::vector removed_stracks; - byte_kalman::KalmanFilter kalman_filter; + KalmanFilter kalman_filter; }; diff --git a/perception/bytetrack/lib/include/kalman_filter.h b/perception/bytetrack/lib/include/kalman_filter.h deleted file mode 100644 index f2b1e1c7817dd..0000000000000 --- a/perception/bytetrack/lib/include/kalman_filter.h +++ /dev/null @@ -1,67 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "data_type.h" - -#include -namespace byte_kalman -{ -class KalmanFilter -{ -public: - static const double chi2inv95[10]; - KalmanFilter(); - KAL_DATA initiate(const DETECTBOX & measurement); - void predict(KAL_MEAN & mean, KAL_COVA & covariance); - KAL_HDATA project(const KAL_MEAN & mean, const KAL_COVA & covariance); - KAL_DATA update( - const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement); - - Eigen::Matrix gating_distance( - const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, - bool only_position = false); - -private: - Eigen::Matrix _motion_mat; - Eigen::Matrix _update_mat; - float _std_weight_position; - float _std_weight_velocity; -}; -} // namespace byte_kalman diff --git a/perception/bytetrack/lib/include/strack.h b/perception/bytetrack/lib/include/strack.h index 2110723e86c58..260f38ea93770 100644 --- a/perception/bytetrack/lib/include/strack.h +++ b/perception/bytetrack/lib/include/strack.h @@ -38,16 +38,18 @@ #pragma once -#include "kalman_filter.h" - +// #include "kalman_filter.h" +#include #include #include +#include #include enum TrackState { New = 0, Tracked, Lost, Removed }; +/** manage one tracklet*/ class STrack { public: @@ -55,8 +57,7 @@ class STrack ~STrack(); std::vector static tlbr_to_tlwh(std::vector & tlbr); - static void multi_predict( - std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter); + static void multi_predict(std::vector & stracks); void static_tlwh(); void static_tlbr(); std::vector tlwh_to_xyah(std::vector tlwh_tmp); @@ -65,10 +66,16 @@ class STrack void mark_removed(); int next_id(); int end_frame(); + void init_kalman_filter(); + void update_kalman_filter(const Eigen::MatrixXd & measurement); + void reflect_state(); - void activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id); + void activate(int frame_id); void re_activate(STrack & new_track, int frame_id, bool new_id = false); void update(STrack & new_track, int frame_id); + void predict(const int frame_id); + + void load_parameters(const std::string & filename); public: bool is_activated; @@ -76,19 +83,41 @@ class STrack boost::uuids::uuid unique_id; int state; - std::vector _tlwh; - std::vector tlwh; - std::vector tlbr; + std::vector original_tlwh; // top left width height + std::vector tlwh; // top left width height + std::vector tlbr; // top left bottom right int frame_id; int tracklet_len; int start_frame; - KAL_MEAN mean; - KAL_COVA covariance; float score; int label; private: - byte_kalman::KalmanFilter kalman_filter; + KalmanFilter kalman_filter_; + struct KfParams + { + // dimension + char dim_x = 8; + char dim_z = 4; + // system noise + float q_cov_x; + float q_cov_y; + float q_cov_vx; + float q_cov_vy; + // measurement noise + float r_cov_x; + float r_cov_y; + // initial state covariance + float p0_cov_x; + float p0_cov_y; + float p0_cov_vx; + float p0_cov_vy; + // other parameters + float dt; // sampling time + }; + static KfParams _kf_parameters; + static bool _parameters_loaded; + enum IDX { X1 = 0, Y1 = 1, X2 = 2, Y2 = 3, VX1 = 4, VY1 = 5, VX2 = 6, VY2 = 7 }; }; diff --git a/perception/bytetrack/lib/src/byte_tracker.cpp b/perception/bytetrack/lib/src/byte_tracker.cpp index 94aae0a5c94cc..a3477e78ad6dd 100644 --- a/perception/bytetrack/lib/src/byte_tracker.cpp +++ b/perception/bytetrack/lib/src/byte_tracker.cpp @@ -106,7 +106,10 @@ std::vector ByteTracker::update(const std::vector & obj ////////////////// Step 2: First association, with IoU ////////////////// strack_pool = joint_stracks(tracked_stracks, this->lost_stracks); - STrack::multi_predict(strack_pool, this->kalman_filter); + // do prediction for each stracks + for (size_t i = 0; i < strack_pool.size(); i++) { + strack_pool[i]->predict(this->frame_id); + } std::vector > dists; int dist_size = 0, dist_size_size = 0; @@ -196,7 +199,7 @@ std::vector ByteTracker::update(const std::vector & obj for (size_t i = 0; i < u_detection.size(); i++) { STrack * track = &detections[u_detection[i]]; if (track->score < this->high_thresh) continue; - track->activate(this->kalman_filter, this->frame_id); + track->activate(this->frame_id); activated_stracks.push_back(*track); } diff --git a/perception/bytetrack/lib/src/kalman_filter.cpp b/perception/bytetrack/lib/src/kalman_filter.cpp deleted file mode 100644 index e515450858d80..0000000000000 --- a/perception/bytetrack/lib/src/kalman_filter.cpp +++ /dev/null @@ -1,175 +0,0 @@ -/* - * MIT License - * - * Copyright (c) 2021 Yifu Zhang - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in all - * copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Copyright 2023 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "kalman_filter.h" - -#include - -namespace byte_kalman -{ -const double KalmanFilter::chi2inv95[10] = {0, 3.8415, 5.9915, 7.8147, 9.4877, - 11.070, 12.592, 14.067, 15.507, 16.919}; -KalmanFilter::KalmanFilter() -{ - int ndim = 4; - double dt = 1.; - - _motion_mat = Eigen::MatrixXf::Identity(8, 8); - for (int i = 0; i < ndim; i++) { - _motion_mat(i, ndim + i) = dt; - } - _update_mat = Eigen::MatrixXf::Identity(4, 8); - - this->_std_weight_position = 1. / 20; - this->_std_weight_velocity = 1. / 160; -} - -KAL_DATA KalmanFilter::initiate(const DETECTBOX & measurement) -{ - DETECTBOX mean_pos = measurement; - DETECTBOX mean_vel; - for (int i = 0; i < 4; i++) mean_vel(i) = 0; - - KAL_MEAN mean; - for (int i = 0; i < 8; i++) { - if (i < 4) - mean(i) = mean_pos(i); - else - mean(i) = mean_vel(i - 4); - } - - KAL_MEAN std; - std(0) = 2 * _std_weight_position * measurement[3]; - std(1) = 2 * _std_weight_position * measurement[3]; - std(2) = 1e-2; - std(3) = 2 * _std_weight_position * measurement[3]; - std(4) = 10 * _std_weight_velocity * measurement[3]; - std(5) = 10 * _std_weight_velocity * measurement[3]; - std(6) = 1e-5; - std(7) = 10 * _std_weight_velocity * measurement[3]; - - KAL_MEAN tmp = std.array().square(); - KAL_COVA var = tmp.asDiagonal(); - return std::make_pair(mean, var); -} - -void KalmanFilter::predict(KAL_MEAN & mean, KAL_COVA & covariance) -{ - // revise the data; - DETECTBOX std_pos; - std_pos << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-2, - _std_weight_position * mean(3); - DETECTBOX std_vel; - std_vel << _std_weight_velocity * mean(3), _std_weight_velocity * mean(3), 1e-5, - _std_weight_velocity * mean(3); - KAL_MEAN tmp; - tmp.block<1, 4>(0, 0) = std_pos; - tmp.block<1, 4>(0, 4) = std_vel; - tmp = tmp.array().square(); - KAL_COVA motion_cov = tmp.asDiagonal(); - KAL_MEAN mean1 = this->_motion_mat * mean.transpose(); - KAL_COVA covariance1 = this->_motion_mat * covariance * (_motion_mat.transpose()); - covariance1 += motion_cov; - - mean = mean1; - covariance = covariance1; -} - -KAL_HDATA KalmanFilter::project(const KAL_MEAN & mean, const KAL_COVA & covariance) -{ - DETECTBOX std; - std << _std_weight_position * mean(3), _std_weight_position * mean(3), 1e-1, - _std_weight_position * mean(3); - KAL_HMEAN mean1 = _update_mat * mean.transpose(); - KAL_HCOVA covariance1 = _update_mat * covariance * (_update_mat.transpose()); - Eigen::Matrix diag = std.asDiagonal(); - diag = diag.array().square().matrix(); - covariance1 += diag; - // covariance1.diagonal() << diag; - return std::make_pair(mean1, covariance1); -} - -KAL_DATA -KalmanFilter::update( - const KAL_MEAN & mean, const KAL_COVA & covariance, const DETECTBOX & measurement) -{ - KAL_HDATA pa = project(mean, covariance); - KAL_HMEAN projected_mean = pa.first; - KAL_HCOVA projected_cov = pa.second; - - // chol_factor, lower = - // scipy.linalg.cho_factor(projected_cov, lower=True, check_finite=False) - // kalmain_gain = - // scipy.linalg.cho_solve((cho_factor, lower), - // np.dot(covariance, self._upadte_mat.T).T, - // check_finite=False).T - Eigen::Matrix B = (covariance * (_update_mat.transpose())).transpose(); - Eigen::Matrix kalman_gain = (projected_cov.llt().solve(B)).transpose(); // eg.8x4 - Eigen::Matrix innovation = measurement - projected_mean; // eg.1x4 - auto tmp = innovation * (kalman_gain.transpose()); - KAL_MEAN new_mean = (mean.array() + tmp.array()).matrix(); - KAL_COVA new_covariance = covariance - kalman_gain * projected_cov * (kalman_gain.transpose()); - return std::make_pair(new_mean, new_covariance); -} - -Eigen::Matrix KalmanFilter::gating_distance( - const KAL_MEAN & mean, const KAL_COVA & covariance, const std::vector & measurements, - bool only_position) -{ - KAL_HDATA pa = this->project(mean, covariance); - if (only_position) { - printf("not implement!"); - exit(0); - } - KAL_HMEAN mean1 = pa.first; - KAL_HCOVA covariance1 = pa.second; - - // Eigen::Matrix d(size, 4); - DETECTBOXSS d(measurements.size(), 4); - int pos = 0; - for (DETECTBOX box : measurements) { - d.row(pos++) = box - mean1; - } - Eigen::Matrix factor = covariance1.llt().matrixL(); - Eigen::Matrix z = - factor.triangularView().solve(d).transpose(); - auto zz = ((z.array()) * (z.array())).matrix(); - auto square_maha = zz.colwise().sum(); - return square_maha; -} -} // namespace byte_kalman diff --git a/perception/bytetrack/lib/src/strack.cpp b/perception/bytetrack/lib/src/strack.cpp index b06456098cc38..eda11509d2403 100644 --- a/perception/bytetrack/lib/src/strack.cpp +++ b/perception/bytetrack/lib/src/strack.cpp @@ -38,12 +38,20 @@ #include "strack.h" +#include + #include -STrack::STrack(std::vector tlwh_, float score, int label) +#include + +// init static variable +bool STrack::_parameters_loaded = false; +STrack::KfParams STrack::_kf_parameters; + +STrack::STrack(std::vector input_tlwh, float score, int label) { - _tlwh.resize(4); - _tlwh.assign(tlwh_.begin(), tlwh_.end()); + original_tlwh.resize(4); + original_tlwh.assign(input_tlwh.begin(), input_tlwh.end()); is_activated = false; track_id = 0; @@ -52,49 +60,71 @@ STrack::STrack(std::vector tlwh_, float score, int label) tlwh.resize(4); tlbr.resize(4); - static_tlwh(); - static_tlbr(); + static_tlwh(); // update object size + static_tlbr(); // update object size frame_id = 0; tracklet_len = 0; this->score = score; start_frame = 0; this->label = label; + + // load static kf parameters: initialized once in program + const std::string package_share_directory = + ament_index_cpp::get_package_share_directory("bytetrack"); + const std::string default_config_path = + package_share_directory + "/config/bytetracker.param.yaml"; + if (!_parameters_loaded) { + load_parameters(default_config_path); + _parameters_loaded = true; + } } STrack::~STrack() { } -void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) +void STrack::init_kalman_filter() +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // init kalman filter state + Eigen::MatrixXd X0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + Eigen::MatrixXd P0 = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + X0(IDX::X1) = this->original_tlwh[0]; + X0(IDX::Y1) = this->original_tlwh[1]; + X0(IDX::X2) = this->original_tlwh[2]; + X0(IDX::Y2) = this->original_tlwh[3]; + P0(IDX::X1, IDX::X1) = _kf_parameters.p0_cov_x; + P0(IDX::Y1, IDX::Y1) = _kf_parameters.p0_cov_y; + P0(IDX::X2, IDX::X2) = _kf_parameters.p0_cov_x; + P0(IDX::Y2, IDX::Y2) = _kf_parameters.p0_cov_y; + P0(IDX::VX1, IDX::VX1) = _kf_parameters.p0_cov_vx; + P0(IDX::VY1, IDX::VY1) = _kf_parameters.p0_cov_vy; + P0(IDX::VX2, IDX::VX2) = _kf_parameters.p0_cov_vx; + P0(IDX::VY2, IDX::VY2) = _kf_parameters.p0_cov_vy; + this->kalman_filter_.init(X0, P0); +} + +/** init a tracklet */ +void STrack::activate(int frame_id) { - this->kalman_filter = kalman_filter; this->track_id = this->next_id(); this->unique_id = boost::uuids::random_generator()(); std::vector _tlwh_tmp(4); - _tlwh_tmp[0] = this->_tlwh[0]; - _tlwh_tmp[1] = this->_tlwh[1]; - _tlwh_tmp[2] = this->_tlwh[2]; - _tlwh_tmp[3] = this->_tlwh[3]; + _tlwh_tmp[0] = this->original_tlwh[0]; + _tlwh_tmp[1] = this->original_tlwh[1]; + _tlwh_tmp[2] = this->original_tlwh[2]; + _tlwh_tmp[3] = this->original_tlwh[3]; std::vector xyah = tlwh_to_xyah(_tlwh_tmp); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.initiate(xyah_box); - this->mean = mc.first; - this->covariance = mc.second; - - static_tlwh(); - static_tlbr(); + // init kf + init_kalman_filter(); + // reflect state + reflect_state(); this->tracklet_len = 0; this->state = TrackState::Tracked; - // if (frame_id == 1) - // { - // this->is_activated = true; - // } this->is_activated = true; this->frame_id = frame_id; this->start_frame = frame_id; @@ -103,17 +133,12 @@ void STrack::activate(byte_kalman::KalmanFilter & kalman_filter, int frame_id) void STrack::re_activate(STrack & new_track, int frame_id, bool new_id) { std::vector xyah = tlwh_to_xyah(new_track.tlwh); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); - this->mean = mc.first; - this->covariance = mc.second; + // TODO(me): write kf update + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.tlwh[0], new_track.tlwh[1], new_track.tlwh[2], new_track.tlwh[3]; + update_kalman_filter(measurement); - static_tlwh(); - static_tlbr(); + reflect_state(); this->tracklet_len = 0; this->state = TrackState::Tracked; @@ -132,18 +157,14 @@ void STrack::update(STrack & new_track, int frame_id) this->tracklet_len++; std::vector xyah = tlwh_to_xyah(new_track.tlwh); - DETECTBOX xyah_box; - xyah_box[0] = xyah[0]; - xyah_box[1] = xyah[1]; - xyah_box[2] = xyah[2]; - xyah_box[3] = xyah[3]; - auto mc = this->kalman_filter.update(this->mean, this->covariance, xyah_box); - this->mean = mc.first; - this->covariance = mc.second; + // update + // TODO(me): write update + Eigen::MatrixXd measurement = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, 1); + measurement << new_track.tlwh[0], new_track.tlwh[1], new_track.tlwh[2], new_track.tlwh[3]; + update_kalman_filter(measurement); - static_tlwh(); - static_tlbr(); + reflect_state(); this->state = TrackState::Tracked; this->is_activated = true; @@ -151,24 +172,31 @@ void STrack::update(STrack & new_track, int frame_id) this->score = new_track.score; } +/** reflect kalman filter state to current object variables*/ +void STrack::reflect_state() +{ + // update tlwh + static_tlwh(); + // update tlbr + static_tlbr(); +} + void STrack::static_tlwh() { if (this->state == TrackState::New) { - tlwh[0] = _tlwh[0]; - tlwh[1] = _tlwh[1]; - tlwh[2] = _tlwh[2]; - tlwh[3] = _tlwh[3]; + tlwh[0] = original_tlwh[0]; + tlwh[1] = original_tlwh[1]; + tlwh[2] = original_tlwh[2]; + tlwh[3] = original_tlwh[3]; return; } - - tlwh[0] = mean[0]; - tlwh[1] = mean[1]; - tlwh[2] = mean[2]; - tlwh[3] = mean[3]; - - tlwh[2] *= tlwh[3]; - tlwh[0] -= tlwh[2] / 2; - tlwh[1] -= tlwh[3] / 2; + // put kf state to tlwh + Eigen::MatrixXd X = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + this->kalman_filter_.getX(X); + tlwh[0] = X(IDX::X1); + tlwh[1] = X(IDX::Y1); + tlwh[2] = X(IDX::X2); + tlwh[3] = X(IDX::Y2); } void STrack::static_tlbr() @@ -222,15 +250,102 @@ int STrack::end_frame() return this->frame_id; } -void STrack::multi_predict( - std::vector & stracks, byte_kalman::KalmanFilter & kalman_filter) +void STrack::multi_predict(std::vector & stracks) { for (size_t i = 0; i < stracks.size(); i++) { if (stracks[i]->state != TrackState::Tracked) { - stracks[i]->mean[7] = 0; + // not tracked } - kalman_filter.predict(stracks[i]->mean, stracks[i]->covariance); + // prediction + stracks[i]->predict(stracks[i]->frame_id + 1); + // TODO(me): write prediction stracks[i]->static_tlwh(); stracks[i]->static_tlbr(); } } + +void STrack::update_kalman_filter(const Eigen::MatrixXd & measurement) +{ + // assert parameter is loaded + assert(_parameters_loaded); + + // get C matrix + Eigen::MatrixXd C = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_x); + C(IDX::X1, IDX::X1) = 1; + C(IDX::Y1, IDX::Y1) = 1; + C(IDX::X2, IDX::X2) = 1; + C(IDX::Y2, IDX::Y2) = 1; + + // get R matrix + Eigen::MatrixXd R = Eigen::MatrixXd::Zero(_kf_parameters.dim_z, _kf_parameters.dim_z); + R(IDX::X1, IDX::X1) = _kf_parameters.r_cov_x; + R(IDX::Y1, IDX::Y1) = _kf_parameters.r_cov_y; + R(IDX::X2, IDX::X2) = _kf_parameters.r_cov_x; + R(IDX::Y2, IDX::Y2) = _kf_parameters.r_cov_y; + + // update + if (!this->kalman_filter_.update(measurement, C, R)) { + std::cerr << "Cannot update" << std::endl; + } +} + +void STrack::predict(const int frame_id) +{ + // check state is Tracked + if (this->state != TrackState::Tracked) { + // not tracked + return; + } + + // else do prediction + float time_elapsed = _kf_parameters.dt * (frame_id - this->frame_id); + // A matrix + Eigen::MatrixXd A = Eigen::MatrixXd::Identity(_kf_parameters.dim_x, _kf_parameters.dim_x); + A(IDX::X1, IDX::VX1) = time_elapsed; + A(IDX::Y1, IDX::VY1) = time_elapsed; + A(IDX::X2, IDX::VX2) = time_elapsed; + A(IDX::Y2, IDX::VY2) = time_elapsed; + + // u and B matrix + Eigen::MatrixXd u = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, 1); + Eigen::MatrixXd B = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + + // get P_t + Eigen::MatrixXd P_t = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + this->kalman_filter_.getP(P_t); + + // Q matrix + Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(_kf_parameters.dim_x, _kf_parameters.dim_x); + Q(IDX::X1, IDX::X1) = _kf_parameters.q_cov_x; + Q(IDX::Y1, IDX::Y1) = _kf_parameters.q_cov_y; + Q(IDX::VX1, IDX::VX1) = _kf_parameters.q_cov_vx; + Q(IDX::VY1, IDX::VY1) = _kf_parameters.q_cov_vy; + Q(IDX::X2, IDX::X2) = _kf_parameters.q_cov_x; + Q(IDX::Y2, IDX::Y2) = _kf_parameters.q_cov_y; + Q(IDX::VX2, IDX::VX2) = _kf_parameters.q_cov_vx; + Q(IDX::VY2, IDX::VY2) = _kf_parameters.q_cov_vy; + + // prediction + if (!this->kalman_filter_.predict(u, A, B, Q)) { + std::cerr << "Cannot predict" << std::endl; + } +} + +void STrack::load_parameters(const std::string & path) +{ + YAML::Node config = YAML::LoadFile(path); + // initialize ekf params + _kf_parameters.dim_x = config["dim_x"].as(); + _kf_parameters.dim_z = config["dim_z"].as(); + _kf_parameters.q_cov_x = config["q_cov_x"].as(); + _kf_parameters.q_cov_y = config["q_cov_y"].as(); + _kf_parameters.q_cov_vx = config["q_cov_vx"].as(); + _kf_parameters.q_cov_vy = config["q_cov_vy"].as(); + _kf_parameters.r_cov_x = config["r_cov_x"].as(); + _kf_parameters.r_cov_y = config["r_cov_y"].as(); + _kf_parameters.p0_cov_x = config["p0_cov_x"].as(); + _kf_parameters.p0_cov_y = config["p0_cov_y"].as(); + _kf_parameters.p0_cov_vx = config["p0_cov_vx"].as(); + _kf_parameters.p0_cov_vy = config["p0_cov_vy"].as(); + _kf_parameters.dt = config["dt"].as(); +} diff --git a/perception/bytetrack/package.xml b/perception/bytetrack/package.xml index 6b3891438c179..da1768e1bf7ea 100644 --- a/perception/bytetrack/package.xml +++ b/perception/bytetrack/package.xml @@ -19,6 +19,7 @@ cv_bridge eigen image_transport + kalman_filter libboost-system-dev libopencv-dev rclcpp @@ -26,6 +27,7 @@ sensor_msgs tensorrt_common tier4_perception_msgs + yaml-cpp ament_lint_auto autoware_lint_common diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index f9b208cca02bc..fe8de37c778ac 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -165,6 +165,9 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets) { lanelet::ConstLanelets intersected_lanelets; + + // WARNING: This implementation currently iterate all lanelets, which could degrade performance + // when handling large sized map. for (const auto & road_lanelet : road_lanelets) { if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) { intersected_lanelets.emplace_back(road_lanelet); diff --git a/perception/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt index 51839027e0e41..3813f1ba4707f 100644 --- a/perception/detection_by_tracker/CMakeLists.txt +++ b/perception/detection_by_tracker/CMakeLists.txt @@ -44,7 +44,8 @@ rclcpp_components_register_node(detection_by_tracker_node EXECUTABLE detection_by_tracker ) -ament_auto_package(INSTALL_TO_SHARE +ament_auto_package( + INSTALL_TO_SHARE launch config ) diff --git a/perception/detection_by_tracker/README.md b/perception/detection_by_tracker/README.md index aece084cd319e..0cff97c4d3ab4 100644 --- a/perception/detection_by_tracker/README.md +++ b/perception/detection_by_tracker/README.md @@ -46,6 +46,17 @@ Simply looking at the overlap between the unknown object and the tracker does no ## Parameters +| Name | Type | Description | Default value | +| --------------------------------- | ------ | --------------------------------------------------------------------- | ------------- | +| `tracker_ignore_label.UNKNOWN` | `bool` | If true, the node will ignore the tracker if its label is unknown. | `true` | +| `tracker_ignore_label.CAR` | `bool` | If true, the node will ignore the tracker if its label is CAR. | `false` | +| `tracker_ignore_label.PEDESTRIAN` | `bool` | If true, the node will ignore the tracker if its label is pedestrian. | `false` | +| `tracker_ignore_label.BICYCLE` | `bool` | If true, the node will ignore the tracker if its label is bicycle. | `false` | +| `tracker_ignore_label.MOTORCYCLE` | `bool` | If true, the node will ignore the tracker if its label is MOTORCYCLE. | `false` | +| `tracker_ignore_label.BUS` | `bool` | If true, the node will ignore the tracker if its label is bus. | `false` | +| `tracker_ignore_label.TRUCK` | `bool` | If true, the node will ignore the tracker if its label is truck. | `false` | +| `tracker_ignore_label.TRAILER` | `bool` | If true, the node will ignore the tracker if its label is TRAILER. | `false` | + ## Assumptions / Known limits ## (Optional) Error detection and handling diff --git a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml index 695704050697d..fac8687c5eeb3 100644 --- a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml +++ b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml @@ -1,11 +1,10 @@ /**: ros__parameters: - tracker_ignore_label: - UNKNOWN : true - CAR : false - TRUCK : false - BUS : false - TRAILER : false - MOTORCYCLE : false - BICYCLE : false - PEDESTRIAN : false + tracker_ignore_label.UNKNOWN : true + tracker_ignore_label.CAR : false + tracker_ignore_label.TRUCK : false + tracker_ignore_label.BUS : false + tracker_ignore_label.TRAILER : false + tracker_ignore_label.MOTORCYCLE : false + tracker_ignore_label.BICYCLE : false + tracker_ignore_label.PEDESTRIAN : false diff --git a/perception/detection_by_tracker/schema/detection_by_tracker.schema.json b/perception/detection_by_tracker/schema/detection_by_tracker.schema.json new file mode 100644 index 0000000000000..22c2b4fa15437 --- /dev/null +++ b/perception/detection_by_tracker/schema/detection_by_tracker.schema.json @@ -0,0 +1,74 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Detection By Tracker Node", + "type": "object", + "definitions": { + "detection_by_tracker": { + "type": "object", + "properties": { + "tracker_ignore_label.UNKNOWN": { + "type": "boolean", + "default": true, + "description": "If true, the node will ignore the tracker if its label is unknown." + }, + "tracker_ignore_label.CAR": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is CAR." + }, + "tracker_ignore_label.PEDESTRIAN": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is pedestrian." + }, + "tracker_ignore_label.BICYCLE": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is bicycle." + }, + "tracker_ignore_label.MOTORCYCLE": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is motorcycle." + }, + "tracker_ignore_label.BUS": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is bus." + }, + "tracker_ignore_label.TRUCK": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is truck." + }, + "tracker_ignore_label.TRAILER": { + "type": "boolean", + "default": false, + "description": "If true, the node will ignore the tracker if its label is TRAILER." + } + }, + "required": [ + "tracker_ignore_label.UNKNOWN", + "tracker_ignore_label.CAR", + "tracker_ignore_label.PEDESTRIAN", + "tracker_ignore_label.BICYCLE", + "tracker_ignore_label.MOTORCYCLE", + "tracker_ignore_label.BUS", + "tracker_ignore_label.TRUCK", + "tracker_ignore_label.TRAILER" + ] + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/detection_by_tracker" + } + }, + "required": ["ros__parameters"] + } + }, + "required": ["/**"] +} diff --git a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md index b15a7c5fef9de..d50a3f97390ca 100644 --- a/perception/image_projection_based_fusion/docs/pointpainting-fusion.md +++ b/perception/image_projection_based_fusion/docs/pointpainting-fusion.md @@ -43,6 +43,7 @@ The lidar points are projected onto the output of an image-only 2d object detect | `encoder_engine_path` | string | `""` | path to VoxelFeatureEncoder TensorRT Engine file | | `head_onnx_path` | string | `""` | path to DetectionHead ONNX file | | `head_engine_path` | string | `""` | path to DetectionHead TensorRT Engine file | +| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp index 5174aebe069a8..9572e62e0e0aa 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/fusion_node.hpp @@ -118,9 +118,10 @@ class FusionNode : public rclcpp::Node // cache for fusion std::vector is_fused_; - std::pair sub_std_pair_; - std::vector> roi_stdmap_; - std::mutex mutex_; + std::pair + cached_msg_; // first element is the timestamp in nanoseconds, second element is the message + std::vector> cached_roi_msgs_; + std::mutex mutex_cached_msgs_; // output publisher typename rclcpp::Publisher::SharedPtr pub_ptr_; diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp index 0f5ada722c31d..d28d9eb31216d 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/pointpainting_trt.hpp @@ -23,6 +23,7 @@ namespace image_projection_based_fusion { +static constexpr size_t CAPACITY_POINT = 1000000; class PointPaintingTRT : public centerpoint::CenterPointTRT { public: @@ -33,8 +34,6 @@ class PointPaintingTRT : public centerpoint::CenterPointTRT const centerpoint::DensificationParam & densification_param, const centerpoint::CenterPointConfig & config); - ~PointPaintingTRT(); - protected: bool preprocess( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp index 7d7f290f71a4c..c913ac33d5e84 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/preprocess_kernel.hpp @@ -20,9 +20,19 @@ namespace image_projection_based_fusion { +cudaError_t generateVoxels_random_launch( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels, + cudaStream_t stream); + +cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream); + cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, + const unsigned int * num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, const std::size_t encoder_in_feature_size, cudaStream_t stream); diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp index 5e7a5087efb84..d0f44b9d58706 100755 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/pointpainting_fusion/voxel_generator.hpp @@ -27,9 +27,7 @@ class VoxelGenerator : public centerpoint::VoxelGenerator public: using centerpoint::VoxelGenerator::VoxelGenerator; - std::size_t pointsToVoxels( - std::vector & voxels, std::vector & coordinates, - std::vector & num_points_per_voxel) override; + std::size_t generateSweepPoints(std::vector & points) override; }; } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp index 199246715551c..de6a8c2987821 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp @@ -64,6 +64,8 @@ bool is_inside( const sensor_msgs::msg::RegionOfInterest & outer, const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale = 1.1); +void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width, const int height); + } // namespace image_projection_based_fusion #endif // IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_ diff --git a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml index 8e79e21db40e3..e15737f5ed222 100644 --- a/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/pointpainting_fusion.launch.xml @@ -24,6 +24,7 @@ + @@ -54,6 +55,7 @@ + diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index df797369208fe..b01a910aaded1 100644 --- a/perception/image_projection_based_fusion/src/fusion_node.cpp +++ b/perception/image_projection_based_fusion/src/fusion_node.cpp @@ -96,7 +96,7 @@ FusionNode::FusionNode( // sub rois rois_subs_.resize(rois_number_); - roi_stdmap_.resize(rois_number_); + cached_roi_msgs_.resize(rois_number_); is_fused_.resize(rois_number_, false); for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) { std::function roi_callback = @@ -163,12 +163,12 @@ void FusionNode::preprocess(Msg & ouput_msg __attribute__((unused))) template void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_msg) { - if (sub_std_pair_.second != nullptr) { + if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); timer_->cancel(); - postprocess(*(sub_std_pair_.second)); - publish(*(sub_std_pair_.second)); - sub_std_pair_.second = nullptr; + postprocess(*(cached_msg_.second)); + publish(*(cached_msg_.second)); + cached_msg_.second = nullptr; std::fill(is_fused_.begin(), is_fused_.end(), false); // add processing time for debug @@ -183,7 +183,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } } - std::lock_guard lock(mutex_); + std::lock_guard lock(mutex_cached_msgs_); auto period = std::chrono::duration_cast( std::chrono::duration(timeout_ms_)); try { @@ -211,12 +211,12 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ continue; } - if ((roi_stdmap_.at(roi_i)).size() > 0) { + if ((cached_roi_msgs_.at(roi_i)).size() > 0) { int64_t min_interval = 1e9; int64_t matched_stamp = -1; std::list outdate_stamps; - for (const auto & [k, v] : roi_stdmap_.at(roi_i)) { + for (const auto & [k, v] : cached_roi_msgs_.at(roi_i)) { int64_t new_stamp = timestamp_nsec + input_offset_ms_.at(roi_i) * (int64_t)1e6; int64_t interval = abs(int64_t(k) - new_stamp); @@ -230,7 +230,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ // remove outdated stamps for (auto stamp : outdate_stamps) { - (roi_stdmap_.at(roi_i)).erase(stamp); + (cached_roi_msgs_.at(roi_i)).erase(stamp); } // fuseOnSingle @@ -240,9 +240,9 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ } fuseOnSingleImage( - *input_msg, roi_i, *((roi_stdmap_.at(roi_i))[matched_stamp]), camera_info_map_.at(roi_i), - *output_msg); - (roi_stdmap_.at(roi_i)).erase(matched_stamp); + *input_msg, roi_i, *((cached_roi_msgs_.at(roi_i))[matched_stamp]), + camera_info_map_.at(roi_i), *output_msg); + (cached_roi_msgs_.at(roi_i)).erase(matched_stamp); is_fused_.at(roi_i) = true; // add timestamp interval for debug @@ -265,7 +265,7 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ postprocess(*output_msg); publish(*output_msg); std::fill(is_fused_.begin(), is_fused_.end(), false); - sub_std_pair_.second = nullptr; + cached_msg_.second = nullptr; // add processing time for debug if (debug_publisher_) { @@ -278,8 +278,8 @@ void FusionNode::subCallback(const typename Msg::ConstSharedPtr input_ processing_time_ms = 0; } } else { - sub_std_pair_.first = int64_t(timestamp_nsec); - sub_std_pair_.second = output_msg; + cached_msg_.first = int64_t(timestamp_nsec); + cached_msg_.second = output_msg; processing_time_ms = stop_watch_ptr_->toc("processing_time", true); } } @@ -294,15 +294,15 @@ void FusionNode::roiCallback( (*input_roi_msg).header.stamp.sec * (int64_t)1e9 + (*input_roi_msg).header.stamp.nanosec; // if cached Msg exist, try to match - if (sub_std_pair_.second != nullptr) { - int64_t new_stamp = sub_std_pair_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; + if (cached_msg_.second != nullptr) { + int64_t new_stamp = cached_msg_.first + input_offset_ms_.at(roi_i) * (int64_t)1e6; int64_t interval = abs(timestamp_nsec - new_stamp); if (interval < match_threshold_ms_ * (int64_t)1e6 && is_fused_.at(roi_i) == false) { if (camera_info_map_.find(roi_i) == camera_info_map_.end()) { RCLCPP_WARN_THROTTLE( this->get_logger(), *this->get_clock(), 5000, "no camera info. id is %zu", roi_i); - (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; + (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; return; } if (debugger_) { @@ -310,12 +310,12 @@ void FusionNode::roiCallback( } fuseOnSingleImage( - *(sub_std_pair_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), - *(sub_std_pair_.second)); + *(cached_msg_.second), roi_i, *input_roi_msg, camera_info_map_.at(roi_i), + *(cached_msg_.second)); is_fused_.at(roi_i) = true; if (debug_publisher_) { - double timestamp_interval_ms = (timestamp_nsec - sub_std_pair_.first) / 1e6; + double timestamp_interval_ms = (timestamp_nsec - cached_msg_.first) / 1e6; debug_publisher_->publish( "debug/roi" + std::to_string(roi_i) + "/timestamp_interval_ms", timestamp_interval_ms); debug_publisher_->publish( @@ -325,10 +325,10 @@ void FusionNode::roiCallback( if (std::count(is_fused_.begin(), is_fused_.end(), true) == static_cast(rois_number_)) { timer_->cancel(); - postprocess(*(sub_std_pair_.second)); - publish(*(sub_std_pair_.second)); + postprocess(*(cached_msg_.second)); + publish(*(cached_msg_.second)); std::fill(is_fused_.begin(), is_fused_.end(), false); - sub_std_pair_.second = nullptr; + cached_msg_.second = nullptr; // add processing time for debug if (debug_publisher_) { @@ -346,7 +346,7 @@ void FusionNode::roiCallback( } } // store roi msg if not matched - (roi_stdmap_.at(roi_i))[timestamp_nsec] = input_roi_msg; + (cached_roi_msgs_.at(roi_i))[timestamp_nsec] = input_roi_msg; } template @@ -360,13 +360,13 @@ void FusionNode::timer_callback() { using std::chrono_literals::operator""ms; timer_->cancel(); - if (mutex_.try_lock()) { + if (mutex_cached_msgs_.try_lock()) { // timeout, postprocess cached msg - if (sub_std_pair_.second != nullptr) { + if (cached_msg_.second != nullptr) { stop_watch_ptr_->toc("processing_time", true); - postprocess(*(sub_std_pair_.second)); - publish(*(sub_std_pair_.second)); + postprocess(*(cached_msg_.second)); + publish(*(cached_msg_.second)); // add processing time for debug if (debug_publisher_) { @@ -380,9 +380,9 @@ void FusionNode::timer_callback() } } std::fill(is_fused_.begin(), is_fused_.end(), false); - sub_std_pair_.second = nullptr; + cached_msg_.second = nullptr; - mutex_.unlock(); + mutex_cached_msgs_.unlock(); } else { try { std::chrono::nanoseconds period = 10ms; diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp index 9f8ef7b94a123..48ef3d26806c8 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/node.cpp @@ -193,6 +193,11 @@ PointPaintingFusionNode::PointPaintingFusionNode(const rclcpp::NodeOptions & opt encoder_param, head_param, densification_param, config); obj_pub_ptr_ = this->create_publisher("~/output/objects", rclcpp::QoS{1}); + + if (this->declare_parameter("build_only", false)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } } void PointPaintingFusionNode::preprocess(sensor_msgs::msg::PointCloud2 & painted_pointcloud_msg) diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp index f0e4de9c988e2..8911442f4c75d 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/pointpainting_trt.cpp @@ -43,26 +43,37 @@ bool PointPaintingTRT::preprocess( if (!is_success) { return false; } - num_voxels_ = vg_ptr_pp_->pointsToVoxels(voxels_, coordinates_, num_points_per_voxel_); - if (num_voxels_ == 0) { - return false; - } - const auto voxels_size = - num_voxels_ * config_.max_point_in_voxel_size_ * config_.point_feature_size_; - const auto coordinates_size = num_voxels_ * config_.point_dim_size_; - // memcpy from host to device (not copy empty voxels) - CHECK_CUDA_ERROR(cudaMemcpyAsync( - voxels_d_.get(), voxels_.data(), voxels_size * sizeof(float), cudaMemcpyHostToDevice)); + const auto count = vg_ptr_pp_->generateSweepPoints(points_); CHECK_CUDA_ERROR(cudaMemcpyAsync( - coordinates_d_.get(), coordinates_.data(), coordinates_size * sizeof(int), - cudaMemcpyHostToDevice)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - num_points_per_voxel_d_.get(), num_points_per_voxel_.data(), num_voxels_ * sizeof(float), - cudaMemcpyHostToDevice)); - CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + points_d_.get(), points_.data(), count * config_.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice, stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_, stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(mask_d_.get(), 0, mask_size_, stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync( + voxels_d_.get(), 0, + config_.max_voxel_size_ * config_.max_point_in_voxel_size_ * config_.point_feature_size_ * + sizeof(float), + stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync( + coordinates_d_.get(), 0, config_.max_voxel_size_ * config_.point_dim_size_ * sizeof(int), + stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync( + num_points_per_voxel_d_.get(), 0, config_.max_voxel_size_ * sizeof(float), stream_)); + + CHECK_CUDA_ERROR(image_projection_based_fusion::generateVoxels_random_launch( + points_d_.get(), count, config_.range_min_x_, config_.range_max_x_, config_.range_min_y_, + config_.range_max_y_, config_.range_min_z_, config_.range_max_z_, config_.voxel_size_x_, + config_.voxel_size_y_, config_.voxel_size_z_, config_.grid_size_y_, config_.grid_size_x_, + mask_d_.get(), voxels_buffer_d_.get(), stream_)); + + CHECK_CUDA_ERROR(image_projection_based_fusion::generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), config_.grid_size_y_, config_.grid_size_x_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_)); CHECK_CUDA_ERROR(image_projection_based_fusion::generateFeatures_launch( - voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_, + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), config_.max_voxel_size_, config_.voxel_size_x_, config_.voxel_size_y_, config_.voxel_size_z_, config_.range_min_x_, config_.range_min_y_, config_.range_min_z_, encoder_in_features_d_.get(), config_.encoder_in_feature_size_, stream_)); diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu index 9789f52de5f74..d06b60633acf8 100644 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/preprocess_kernel.cu @@ -57,9 +57,105 @@ std::size_t divup(const std::size_t a, const std::size_t b) namespace image_projection_based_fusion { +__global__ void generateVoxels_random_kernel( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + float x = points[point_idx * POINT_FEATURE_SIZE]; + float y = points[point_idx * POINT_FEATURE_SIZE + 1]; + float z = points[point_idx * POINT_FEATURE_SIZE + 2]; + + if ( + x < min_x_range || x >= max_x_range || y < min_y_range || y >= max_y_range || z < min_z_range || + z >= max_z_range) + return; + + int voxel_idx = floorf((x - min_x_range) / pillar_x_size); + int voxel_idy = floorf((y - min_y_range) / pillar_y_size); + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + + unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); + + if (point_id >= MAX_POINT_IN_VOXEL_SIZE) return; + float * address = + voxels + (voxel_index * MAX_POINT_IN_VOXEL_SIZE + point_id) * POINT_FEATURE_SIZE; + for (unsigned int i = 0; i < POINT_FEATURE_SIZE; ++i) { + atomicExch(address + i, points[point_idx * POINT_FEATURE_SIZE + i]); + } +} + +cudaError_t generateVoxels_random_launch( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels, + cudaStream_t stream) +{ + dim3 blocks((points_size + 256 - 1) / 256); + dim3 threads(256); + generateVoxels_random_kernel<<>>( + points, points_size, min_x_range, max_x_range, min_y_range, max_y_range, min_z_range, + max_z_range, pillar_x_size, pillar_y_size, pillar_z_size, grid_y_size, grid_x_size, mask, + voxels); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateBaseFeatures_kernel( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs) +{ + unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; + unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; + + if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return; + + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + unsigned int count = mask[voxel_index]; + if (!(count > 0)) return; + count = count < MAX_POINT_IN_VOXEL_SIZE ? count : MAX_POINT_IN_VOXEL_SIZE; + + unsigned int current_pillarId = 0; + current_pillarId = atomicAdd(pillar_num, 1); + + voxel_num[current_pillarId] = count; + + uint3 idx = {0, voxel_idy, voxel_idx}; + ((uint3 *)voxel_idxs)[current_pillarId] = idx; + + for (int i = 0; i < count; i++) { + int inIndex = voxel_index * MAX_POINT_IN_VOXEL_SIZE + i; + int outIndex = current_pillarId * MAX_POINT_IN_VOXEL_SIZE + i; + for (unsigned int j = 0; j < POINT_FEATURE_SIZE; ++j) { + voxel_features[outIndex * POINT_FEATURE_SIZE + j] = voxels[inIndex * POINT_FEATURE_SIZE + j]; + } + } + + // clear buffer for next infer + atomicExch(mask + voxel_index, 0); +} + +// create 4 channels +cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream) +{ + dim3 threads = {32, 32}; + dim3 blocks = { + (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y}; + + generateBaseFeatures_kernel<<>>( + mask, voxels, grid_y_size, grid_x_size, pillar_num, voxel_features, voxel_num, voxel_idxs); + cudaError_t err = cudaGetLastError(); + return err; +} + __global__ void generateFeatures_kernel( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, + const unsigned int * num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, const std::size_t encoder_in_feature_size) { @@ -70,7 +166,8 @@ __global__ void generateFeatures_kernel( int point_idx = threadIdx.x % MAX_POINT_IN_VOXEL_SIZE; int pillar_idx_inBlock = threadIdx.x / MAX_POINT_IN_VOXEL_SIZE; - if (pillar_idx >= num_voxels) return; + unsigned int num_pillars = num_voxels[0]; + if (pillar_idx >= num_pillars) return; // load src __shared__ float pillarSM[WARPS_PER_BLOCK][MAX_POINT_IN_VOXEL_SIZE][POINT_FEATURE_SIZE]; @@ -155,7 +252,7 @@ __global__ void generateFeatures_kernel( cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, + const unsigned int * num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, const std::size_t encoder_in_feature_size, cudaStream_t stream) diff --git a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp index f1ba163ee69b9..ea10cb1237436 100755 --- a/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp +++ b/perception/image_projection_based_fusion/src/pointpainting_fusion/voxel_generator.cpp @@ -18,29 +18,10 @@ namespace image_projection_based_fusion { -std::size_t VoxelGenerator::pointsToVoxels( - std::vector & voxels, std::vector & coordinates, - std::vector & num_points_per_voxel) +size_t VoxelGenerator::generateSweepPoints(std::vector & points) { - // voxels (float): (max_num_voxels * max_num_points_per_voxel * point_feature_size) - // coordinates (int): (max_num_voxels * point_dim_size) - // num_points_per_voxel (float): (max_num_voxels) - - const std::size_t grid_size = config_.grid_size_z_ * config_.grid_size_y_ * config_.grid_size_x_; - std::vector> coord_to_voxel_idx(grid_size, -1); - - std::size_t voxel_cnt = 0; // @return - // std::array point; - // std::array coord_zyx; - std::vector point; - point.resize(config_.point_feature_size_); - std::vector coord_zyx; - coord_zyx.resize(config_.point_dim_size_); - bool out_of_range; - std::size_t point_cnt; - int c, coord_idx, voxel_idx; Eigen::Vector3f point_current, point_past; - + size_t point_counter{}; for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); pc_cache_iter++) { auto pc_msg = pc_cache_iter->pointcloud_msg; @@ -55,62 +36,25 @@ std::size_t VoxelGenerator::pointsToVoxels( point_past << *x_iter, *y_iter, *z_iter; point_current = affine_past2current * point_past; - point[0] = point_current.x(); - point[1] = point_current.y(); - point[2] = point_current.z(); - point[3] = time_lag; + points.at(point_counter * config_.point_feature_size_) = point_current.x(); + points.at(point_counter * config_.point_feature_size_ + 1) = point_current.y(); + points.at(point_counter * config_.point_feature_size_ + 2) = point_current.z(); + points.at(point_counter * config_.point_feature_size_ + 3) = time_lag; // decode the class value back to one-hot binary and assign it to point - std::fill(point.begin() + 4, point.end(), 0); + for (size_t i = 0; i < config_.class_size_; ++i) { + points.at(point_counter * config_.point_feature_size_ + 4 + i) = 0.f; + } auto class_value = static_cast(*class_iter); - auto iter = point.begin() + 4; + auto iter = points.begin() + point_counter * config_.point_feature_size_ + 4; while (class_value > 0) { *iter = class_value % 2; class_value /= 2; ++iter; } - - out_of_range = false; - for (std::size_t di = 0; di < config_.point_dim_size_; di++) { - c = static_cast((point[di] - range_[di]) * recip_voxel_size_[di]); - if (c < 0 || c >= grid_size_[di]) { - out_of_range = true; - break; - } - coord_zyx[config_.point_dim_size_ - di - 1] = c; - } - if (out_of_range) { - continue; - } - - coord_idx = coord_zyx[0] * config_.grid_size_y_ * config_.grid_size_x_ + - coord_zyx[1] * config_.grid_size_x_ + coord_zyx[2]; - voxel_idx = coord_to_voxel_idx[coord_idx].value(); - if (voxel_idx == -1) { - voxel_idx = voxel_cnt; - if (voxel_cnt >= config_.max_voxel_size_) { - continue; - } - - voxel_cnt++; - coord_to_voxel_idx[coord_idx] = voxel_idx; - for (std::size_t di = 0; di < config_.point_dim_size_; di++) { - coordinates[voxel_idx * config_.point_dim_size_ + di] = coord_zyx[di]; - } - } - - point_cnt = num_points_per_voxel[voxel_idx]; - if (point_cnt < config_.max_point_in_voxel_size_) { - for (std::size_t fi = 0; fi < config_.point_feature_size_; fi++) { - voxels - [voxel_idx * config_.max_point_in_voxel_size_ * config_.point_feature_size_ + - point_cnt * config_.point_feature_size_ + fi] = point[fi]; - } - num_points_per_voxel[voxel_idx]++; - } + ++point_counter; } } - - return voxel_cnt; + return point_counter; } } // namespace image_projection_based_fusion diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp index 9da9df7eff6ca..492f832e30e8b 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -177,17 +177,18 @@ void RoiClusterFusionNode::fuseOnSingleImage( double iou(0.0); bool is_use_non_trust_object_iou_mode = is_far_enough( input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_); + auto image_roi = feature_obj.feature.roi; + auto cluster_roi = cluster_map.second; + sanitizeROI(image_roi, camera_info.width, camera_info.height); + sanitizeROI(cluster_roi, camera_info.width, camera_info.height); if (is_use_non_trust_object_iou_mode || is_roi_label_known) { - iou = - cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_); + iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_); } else { - iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_); + iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_); } const bool passed_inside_cluster_gate = - only_allow_inside_cluster_ - ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_) - : true; + only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true; if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; max_iou = iou; diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp index 9ea49256650ee..29198280ec7b2 100644 --- a/perception/image_projection_based_fusion/src/utils/geometry.cpp +++ b/perception/image_projection_based_fusion/src/utils/geometry.cpp @@ -166,11 +166,42 @@ bool is_inside( const sensor_msgs::msg::RegionOfInterest & outer, const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale) { - const double lower_scale = 1.0 - std::abs(outer_offset_scale - 1.0); - return outer.x_offset * lower_scale <= inner.x_offset && - outer.y_offset * lower_scale <= inner.y_offset && - inner.x_offset + inner.width <= (outer.x_offset + outer.width) * outer_offset_scale && - inner.y_offset + inner.height <= (outer.y_offset + outer.height) * outer_offset_scale; + const auto scaled_width = static_cast(outer.width) * outer_offset_scale; + const auto scaled_height = static_cast(outer.height) * outer_offset_scale; + const auto scaled_x_offset = + static_cast(outer.x_offset) - (scaled_width - outer.width) / 2.0; + const auto scaled_y_offset = + static_cast(outer.y_offset) - (scaled_height - outer.height) / 2.0; + + // 1. check left-top corner + if (scaled_x_offset > inner.x_offset || scaled_y_offset > inner.y_offset) { + return false; + } + // 2. check right-bottom corner + if ( + scaled_x_offset + scaled_width < inner.x_offset + inner.width || + scaled_y_offset + scaled_height < inner.y_offset + inner.height) { + return false; + } + return true; +} + +void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width_, const int height_) +{ + const unsigned int width = static_cast(width_); + const unsigned int height = static_cast(height_); + if (roi.x_offset >= width || roi.y_offset >= height) { + roi.width = 0; + roi.height = 0; + return; + } + + if (roi.x_offset + roi.width > width) { + roi.width = width - roi.x_offset; + } + if (roi.y_offset + roi.height > height) { + roi.height = height - roi.y_offset; + } } } // namespace image_projection_based_fusion diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index fee17f1c0156a..8cf250be0c049 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -32,6 +32,8 @@ namespace centerpoint { +static constexpr size_t CAPACITY_POINT = 1000000; + class NetworkParam { public: @@ -59,7 +61,7 @@ class CenterPointTRT const NetworkParam & encoder_param, const NetworkParam & head_param, const DensificationParam & densification_param, const CenterPointConfig & config); - ~CenterPointTRT(); + virtual ~CenterPointTRT(); bool detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, @@ -83,12 +85,13 @@ class CenterPointTRT std::size_t class_size_{0}; CenterPointConfig config_; - std::size_t num_voxels_{0}; std::size_t encoder_in_feature_size_{0}; std::size_t spatial_features_size_{0}; - std::vector voxels_; - std::vector coordinates_; - std::vector num_points_per_voxel_; + std::size_t voxels_buffer_size_{0}; + std::size_t mask_size_{0}; + std::size_t voxels_size_{0}; + std::size_t coordinates_size_{0}; + std::vector points_; cuda::unique_ptr voxels_d_{nullptr}; cuda::unique_ptr coordinates_d_{nullptr}; cuda::unique_ptr num_points_per_voxel_d_{nullptr}; @@ -101,6 +104,10 @@ class CenterPointTRT cuda::unique_ptr head_out_dim_d_{nullptr}; cuda::unique_ptr head_out_rot_d_{nullptr}; cuda::unique_ptr head_out_vel_d_{nullptr}; + cuda::unique_ptr points_d_{nullptr}; + cuda::unique_ptr voxels_buffer_d_{nullptr}; + cuda::unique_ptr mask_d_{nullptr}; + cuda::unique_ptr num_voxels_d_{nullptr}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp index e754d89468ba3..3dd00c25fd9e7 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/scatter_kernel.hpp @@ -21,7 +21,7 @@ namespace centerpoint { cudaError_t scatterFeatures_launch( - const float * pillar_features, const int * coords, const std::size_t num_pillars, + const float * pillar_features, const int * coords, const unsigned int * num_pillars, const std::size_t max_voxel_size, const std::size_t encoder_out_feature_size, const std::size_t grid_size_x, const std::size_t grid_size_y, float * scattered_features, cudaStream_t stream); diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp index 3abc8cfeccd5b..824144fe3b22b 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/preprocess_kernel.hpp @@ -20,9 +20,19 @@ namespace centerpoint { +cudaError_t generateVoxels_random_launch( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels, + cudaStream_t stream); + +cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream); + cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, + const unsigned int * num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, cudaStream_t stream); diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp index 810ae3afd20d0..be15d51e91715 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/preprocess/voxel_generator.hpp @@ -31,9 +31,7 @@ class VoxelGeneratorTemplate explicit VoxelGeneratorTemplate( const DensificationParam & param, const CenterPointConfig & config); - virtual std::size_t pointsToVoxels( - std::vector & voxels, std::vector & coordinates, - std::vector & num_points_per_voxel) = 0; + virtual std::size_t generateSweepPoints(std::vector & points) = 0; bool enqueuePointCloud( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer); @@ -52,9 +50,7 @@ class VoxelGenerator : public VoxelGeneratorTemplate public: using VoxelGeneratorTemplate::VoxelGeneratorTemplate; - std::size_t pointsToVoxels( - std::vector & voxels, std::vector & coordinates, - std::vector & num_points_per_voxel) override; + std::size_t generateSweepPoints(std::vector & points) override; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index 53e19500ff428..67271985d3b5e 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -69,9 +69,9 @@ CenterPointTRT::~CenterPointTRT() void CenterPointTRT::initPtr() { - const auto voxels_size = + voxels_size_ = config_.max_voxel_size_ * config_.max_point_in_voxel_size_ * config_.point_feature_size_; - const auto coordinates_size = config_.max_voxel_size_ * config_.point_dim_size_; + coordinates_size_ = config_.max_voxel_size_ * config_.point_dim_size_; encoder_in_feature_size_ = config_.max_voxel_size_ * config_.max_point_in_voxel_size_ * config_.encoder_in_feature_size_; const auto pillar_features_size = config_.max_voxel_size_ * config_.encoder_out_feature_size_; @@ -79,14 +79,16 @@ void CenterPointTRT::initPtr() config_.grid_size_x_ * config_.grid_size_y_ * config_.encoder_out_feature_size_; const auto grid_xy_size = config_.down_grid_size_x_ * config_.down_grid_size_y_; + voxels_buffer_size_ = config_.grid_size_x_ * config_.grid_size_y_ * + config_.max_point_in_voxel_size_ * config_.point_feature_size_; + mask_size_ = config_.grid_size_x_ * config_.grid_size_y_; + // host - voxels_.resize(voxels_size); - coordinates_.resize(coordinates_size); - num_points_per_voxel_.resize(config_.max_voxel_size_); + points_.resize(CAPACITY_POINT * config_.point_feature_size_); // device - voxels_d_ = cuda::make_unique(voxels_size); - coordinates_d_ = cuda::make_unique(coordinates_size); + voxels_d_ = cuda::make_unique(voxels_size_); + coordinates_d_ = cuda::make_unique(coordinates_size_); num_points_per_voxel_d_ = cuda::make_unique(config_.max_voxel_size_); encoder_in_features_d_ = cuda::make_unique(encoder_in_feature_size_); pillar_features_d_ = cuda::make_unique(pillar_features_size); @@ -97,15 +99,16 @@ void CenterPointTRT::initPtr() head_out_dim_d_ = cuda::make_unique(grid_xy_size * config_.head_out_dim_size_); head_out_rot_d_ = cuda::make_unique(grid_xy_size * config_.head_out_rot_size_); head_out_vel_d_ = cuda::make_unique(grid_xy_size * config_.head_out_vel_size_); + points_d_ = cuda::make_unique(CAPACITY_POINT * config_.point_feature_size_); + voxels_buffer_d_ = cuda::make_unique(voxels_buffer_size_); + mask_d_ = cuda::make_unique(mask_size_); + num_voxels_d_ = cuda::make_unique(1); } bool CenterPointTRT::detect( const sensor_msgs::msg::PointCloud2 & input_pointcloud_msg, const tf2_ros::Buffer & tf_buffer, std::vector & det_boxes3d) { - std::fill(voxels_.begin(), voxels_.end(), 0); - std::fill(coordinates_.begin(), coordinates_.end(), -1); - std::fill(num_points_per_voxel_.begin(), num_points_per_voxel_.end(), 0); CHECK_CUDA_ERROR(cudaMemsetAsync( encoder_in_features_d_.get(), 0, encoder_in_feature_size_ * sizeof(float), stream_)); CHECK_CUDA_ERROR( @@ -131,27 +134,33 @@ bool CenterPointTRT::preprocess( if (!is_success) { return false; } - num_voxels_ = vg_ptr_->pointsToVoxels(voxels_, coordinates_, num_points_per_voxel_); - if (num_voxels_ == 0) { - return false; - } - - const auto voxels_size = - num_voxels_ * config_.max_point_in_voxel_size_ * config_.point_feature_size_; - const auto coordinates_size = num_voxels_ * config_.point_dim_size_; - // memcpy from host to device (not copy empty voxels) - CHECK_CUDA_ERROR(cudaMemcpyAsync( - voxels_d_.get(), voxels_.data(), voxels_size * sizeof(float), cudaMemcpyHostToDevice)); - CHECK_CUDA_ERROR(cudaMemcpyAsync( - coordinates_d_.get(), coordinates_.data(), coordinates_size * sizeof(int), - cudaMemcpyHostToDevice)); + const auto count = vg_ptr_->generateSweepPoints(points_); CHECK_CUDA_ERROR(cudaMemcpyAsync( - num_points_per_voxel_d_.get(), num_points_per_voxel_.data(), num_voxels_ * sizeof(float), - cudaMemcpyHostToDevice)); - CHECK_CUDA_ERROR(cudaStreamSynchronize(stream_)); + points_d_.get(), points_.data(), count * config_.point_feature_size_ * sizeof(float), + cudaMemcpyHostToDevice, stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(num_voxels_d_.get(), 0, sizeof(unsigned int), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(voxels_buffer_d_.get(), 0, voxels_buffer_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(mask_d_.get(), 0, mask_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync(voxels_d_.get(), 0, voxels_size_ * sizeof(float), stream_)); + CHECK_CUDA_ERROR( + cudaMemsetAsync(coordinates_d_.get(), 0, coordinates_size_ * sizeof(int), stream_)); + CHECK_CUDA_ERROR(cudaMemsetAsync( + num_points_per_voxel_d_.get(), 0, config_.max_voxel_size_ * sizeof(float), stream_)); + + CHECK_CUDA_ERROR(generateVoxels_random_launch( + points_d_.get(), count, config_.range_min_x_, config_.range_max_x_, config_.range_min_y_, + config_.range_max_y_, config_.range_min_z_, config_.range_max_z_, config_.voxel_size_x_, + config_.voxel_size_y_, config_.voxel_size_z_, config_.grid_size_y_, config_.grid_size_x_, + mask_d_.get(), voxels_buffer_d_.get(), stream_)); + + CHECK_CUDA_ERROR(generateBaseFeatures_launch( + mask_d_.get(), voxels_buffer_d_.get(), config_.grid_size_y_, config_.grid_size_x_, + num_voxels_d_.get(), voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), + stream_)); CHECK_CUDA_ERROR(generateFeatures_launch( - voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_, + voxels_d_.get(), num_points_per_voxel_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), config_.max_voxel_size_, config_.voxel_size_x_, config_.voxel_size_y_, config_.voxel_size_z_, config_.range_min_x_, config_.range_min_y_, config_.range_min_z_, encoder_in_features_d_.get(), stream_)); @@ -171,7 +180,7 @@ void CenterPointTRT::inference() // scatter CHECK_CUDA_ERROR(scatterFeatures_launch( - pillar_features_d_.get(), coordinates_d_.get(), num_voxels_, config_.max_voxel_size_, + pillar_features_d_.get(), coordinates_d_.get(), num_voxels_d_.get(), config_.max_voxel_size_, config_.encoder_out_feature_size_, config_.grid_size_x_, config_.grid_size_y_, spatial_features_d_.get(), stream_)); diff --git a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu index da2f0e2f57c15..09cc83bf606fe 100644 --- a/perception/lidar_centerpoint/lib/network/scatter_kernel.cu +++ b/perception/lidar_centerpoint/lib/network/scatter_kernel.cu @@ -24,7 +24,7 @@ const std::size_t THREADS_PER_BLOCK = 32; namespace centerpoint { __global__ void scatterFeatures_kernel( - const float * pillar_features, const int * coords, const std::size_t num_pillars, + const float * pillar_features, const int * coords, const unsigned int * num_pillars, const std::size_t pillar_feature_size, const std::size_t grid_size_x, const std::size_t grid_size_y, float * scattered_features) { @@ -34,7 +34,7 @@ __global__ void scatterFeatures_kernel( const auto pillar_i = blockIdx.x * THREADS_PER_BLOCK + threadIdx.x; const auto feature_i = blockIdx.y * THREADS_PER_BLOCK + threadIdx.y; - if (pillar_i >= num_pillars || feature_i >= pillar_feature_size) { + if (pillar_i >= num_pillars[0] || feature_i >= pillar_feature_size) { return; } @@ -50,7 +50,7 @@ __global__ void scatterFeatures_kernel( // cspell: ignore divup cudaError_t scatterFeatures_launch( - const float * pillar_features, const int * coords, const std::size_t num_pillars, + const float * pillar_features, const int * coords, const unsigned int * num_pillars, const std::size_t max_voxel_size, const std::size_t encoder_out_feature_size, const std::size_t grid_size_x, const std::size_t grid_size_y, float * scattered_features, cudaStream_t stream) diff --git a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu index 7b59757311ff2..6f77ff84c2cea 100644 --- a/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu +++ b/perception/lidar_centerpoint/lib/preprocess/preprocess_kernel.cu @@ -41,9 +41,101 @@ const std::size_t ENCODER_IN_FEATURE_SIZE = 9; // the same as encoder_in_featur namespace centerpoint { +__global__ void generateVoxels_random_kernel( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels) +{ + int point_idx = blockIdx.x * blockDim.x + threadIdx.x; + if (point_idx >= points_size) return; + + float4 point = ((float4 *)points)[point_idx]; + + if ( + point.x < min_x_range || point.x >= max_x_range || point.y < min_y_range || + point.y >= max_y_range || point.z < min_z_range || point.z >= max_z_range) + return; + + int voxel_idx = floorf((point.x - min_x_range) / pillar_x_size); + int voxel_idy = floorf((point.y - min_y_range) / pillar_y_size); + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + + unsigned int point_id = atomicAdd(&(mask[voxel_index]), 1); + + if (point_id >= MAX_POINT_IN_VOXEL_SIZE) return; + float * address = voxels + (voxel_index * MAX_POINT_IN_VOXEL_SIZE + point_id) * 4; + atomicExch(address + 0, point.x); + atomicExch(address + 1, point.y); + atomicExch(address + 2, point.z); + atomicExch(address + 3, point.w); +} + +cudaError_t generateVoxels_random_launch( + const float * points, size_t points_size, float min_x_range, float max_x_range, float min_y_range, + float max_y_range, float min_z_range, float max_z_range, float pillar_x_size, float pillar_y_size, + float pillar_z_size, int grid_y_size, int grid_x_size, unsigned int * mask, float * voxels, + cudaStream_t stream) +{ + dim3 blocks((points_size + 256 - 1) / 256); + dim3 threads(256); + generateVoxels_random_kernel<<>>( + points, points_size, min_x_range, max_x_range, min_y_range, max_y_range, min_z_range, + max_z_range, pillar_x_size, pillar_y_size, pillar_z_size, grid_y_size, grid_x_size, mask, + voxels); + cudaError_t err = cudaGetLastError(); + return err; +} + +__global__ void generateBaseFeatures_kernel( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs) +{ + unsigned int voxel_idx = blockIdx.x * blockDim.x + threadIdx.x; + unsigned int voxel_idy = blockIdx.y * blockDim.y + threadIdx.y; + + if (voxel_idx >= grid_x_size || voxel_idy >= grid_y_size) return; + + unsigned int voxel_index = voxel_idy * grid_x_size + voxel_idx; + unsigned int count = mask[voxel_index]; + if (!(count > 0)) return; + count = count < MAX_POINT_IN_VOXEL_SIZE ? count : MAX_POINT_IN_VOXEL_SIZE; + + unsigned int current_pillarId = 0; + current_pillarId = atomicAdd(pillar_num, 1); + + voxel_num[current_pillarId] = count; + + uint3 idx = {0, voxel_idy, voxel_idx}; + ((uint3 *)voxel_idxs)[current_pillarId] = idx; + + for (int i = 0; i < count; i++) { + int inIndex = voxel_index * MAX_POINT_IN_VOXEL_SIZE + i; + int outIndex = current_pillarId * MAX_POINT_IN_VOXEL_SIZE + i; + ((float4 *)voxel_features)[outIndex] = ((float4 *)voxels)[inIndex]; + } + + // clear buffer for next infer + atomicExch(mask + voxel_index, 0); +} + +// create 4 channels +cudaError_t generateBaseFeatures_launch( + unsigned int * mask, float * voxels, int grid_y_size, int grid_x_size, unsigned int * pillar_num, + float * voxel_features, float * voxel_num, int * voxel_idxs, cudaStream_t stream) +{ + dim3 threads = {32, 32}; + dim3 blocks = { + (grid_x_size + threads.x - 1) / threads.x, (grid_y_size + threads.y - 1) / threads.y}; + + generateBaseFeatures_kernel<<>>( + mask, voxels, grid_y_size, grid_x_size, pillar_num, voxel_features, voxel_num, voxel_idxs); + cudaError_t err = cudaGetLastError(); + return err; +} + __global__ void generateFeatures_kernel( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, + const unsigned int * num_voxels, const float voxel_x, const float voxel_y, const float voxel_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features) { // voxel_features (float): (max_voxel_size, max_point_in_voxel_size, point_feature_size) @@ -53,7 +145,8 @@ __global__ void generateFeatures_kernel( int point_idx = threadIdx.x % MAX_POINT_IN_VOXEL_SIZE; int pillar_idx_inBlock = threadIdx.x / MAX_POINT_IN_VOXEL_SIZE; // max_point_in_voxel_size - if (pillar_idx >= num_voxels) return; + unsigned int num_pillars = num_voxels[0]; + if (pillar_idx >= num_pillars) return; // load src __shared__ float4 pillarSM[WARPS_PER_BLOCK][MAX_POINT_IN_VOXEL_SIZE]; @@ -144,7 +237,7 @@ __global__ void generateFeatures_kernel( // cspell: ignore divup cudaError_t generateFeatures_launch( const float * voxel_features, const float * voxel_num_points, const int * coords, - const std::size_t num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, + const unsigned int * num_voxels, const std::size_t max_voxel_size, const float voxel_size_x, const float voxel_size_y, const float voxel_size_z, const float range_min_x, const float range_min_y, const float range_min_z, float * features, cudaStream_t stream) { diff --git a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp index 7f4e4a849211c..07a1a2de725f5 100644 --- a/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp +++ b/perception/lidar_centerpoint/lib/preprocess/voxel_generator.cpp @@ -43,34 +43,15 @@ bool VoxelGeneratorTemplate::enqueuePointCloud( return pd_ptr_->enqueuePointCloud(input_pointcloud_msg, tf_buffer); } -std::size_t VoxelGenerator::pointsToVoxels( - std::vector & voxels, std::vector & coordinates, - std::vector & num_points_per_voxel) +std::size_t VoxelGenerator::generateSweepPoints(std::vector & points) { - // voxels (float): (max_voxel_size * max_point_in_voxel_size * point_feature_size) - // coordinates (int): (max_voxel_size * point_dim_size) - // num_points_per_voxel (float): (max_voxel_size) - - const std::size_t grid_size = config_.grid_size_z_ * config_.grid_size_y_ * config_.grid_size_x_; - std::vector coord_to_voxel_idx(grid_size, -1); - - std::size_t voxel_cnt = 0; // @return - std::vector point; - point.resize(config_.point_feature_size_); - std::vector coord_zyx; - coord_zyx.resize(config_.point_dim_size_); - bool out_of_range; - std::size_t point_cnt; - int c, coord_idx, voxel_idx; Eigen::Vector3f point_current, point_past; - + size_t point_counter{}; for (auto pc_cache_iter = pd_ptr_->getPointCloudCacheIter(); !pd_ptr_->isCacheEnd(pc_cache_iter); pc_cache_iter++) { auto pc_msg = pc_cache_iter->pointcloud_msg; auto affine_past2current = - pd_ptr_->pointcloud_cache_size() > 1 - ? pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world - : Eigen::Affine3f::Identity(); + pd_ptr_->getAffineWorldToCurrent() * pc_cache_iter->affine_past2world; float time_lag = static_cast( pd_ptr_->getCurrentTimestamp() - rclcpp::Time(pc_msg.header.stamp).seconds()); @@ -80,53 +61,14 @@ std::size_t VoxelGenerator::pointsToVoxels( point_past << *x_iter, *y_iter, *z_iter; point_current = affine_past2current * point_past; - point[0] = point_current.x(); - point[1] = point_current.y(); - point[2] = point_current.z(); - point[3] = time_lag; - - out_of_range = false; - for (std::size_t di = 0; di < config_.point_dim_size_; di++) { - c = static_cast((point[di] - range_[di]) * recip_voxel_size_[di]); - if (c < 0 || c >= grid_size_[di]) { - out_of_range = true; - break; - } - coord_zyx[config_.point_dim_size_ - di - 1] = c; - } - if (out_of_range) { - continue; - } - - coord_idx = coord_zyx[0] * config_.grid_size_y_ * config_.grid_size_x_ + - coord_zyx[1] * config_.grid_size_x_ + coord_zyx[2]; - voxel_idx = coord_to_voxel_idx[coord_idx]; - if (voxel_idx == -1) { - voxel_idx = voxel_cnt; - if (voxel_cnt >= config_.max_voxel_size_) { - continue; - } - - voxel_cnt++; - coord_to_voxel_idx[coord_idx] = voxel_idx; - for (std::size_t di = 0; di < config_.point_dim_size_; di++) { - coordinates[voxel_idx * config_.point_dim_size_ + di] = coord_zyx[di]; - } - } - - point_cnt = num_points_per_voxel[voxel_idx]; - if (point_cnt < config_.max_point_in_voxel_size_) { - for (std::size_t fi = 0; fi < config_.point_feature_size_; fi++) { - voxels - [voxel_idx * config_.max_point_in_voxel_size_ * config_.point_feature_size_ + - point_cnt * config_.point_feature_size_ + fi] = point[fi]; - } - num_points_per_voxel[voxel_idx]++; - } + points.at(point_counter * config_.point_feature_size_) = point_current.x(); + points.at(point_counter * config_.point_feature_size_ + 1) = point_current.y(); + points.at(point_counter * config_.point_feature_size_ + 2) = point_current.z(); + points.at(point_counter * config_.point_feature_size_ + 3) = time_lag; + ++point_counter; } } - - return voxel_cnt; + return point_counter; } } // namespace centerpoint diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 53c78b54bc2ab..9378e09f099cc 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -6,6 +6,8 @@ autoware_package() find_package(Eigen3 REQUIRED) +find_package(glog REQUIRED) + include_directories( SYSTEM ${EIGEN3_INCLUDE_DIR} @@ -17,6 +19,8 @@ ament_auto_add_library(map_based_prediction_node SHARED src/debug.cpp ) +target_link_libraries(map_based_prediction_node glog::glog) + rclcpp_components_register_node(map_based_prediction_node PLUGIN "map_based_prediction::MapBasedPredictionNode" EXECUTABLE map_based_prediction diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 260ae45da2e05..69ff96a263f4b 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -29,5 +29,6 @@ diff_dist_threshold_to_left_bound: 0.29 #[m] diff_dist_threshold_to_right_bound: -0.29 #[m] num_continuous_state_transition: 3 + consider_only_routable_neighbours: false reference_path_resolution: 0.5 #[m] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 01f39057aef36..bdd9ad89bc025 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -157,6 +157,7 @@ class MapBasedPredictionNode : public rclcpp::Node double diff_dist_threshold_to_left_bound_; double diff_dist_threshold_to_right_bound_; int num_continuous_state_transition_; + bool consider_only_routable_neighbours_; double reference_path_resolution_; // Stop watch diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 6a1354b37928f..b07d9855f9821 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -18,6 +18,7 @@ autoware_auto_perception_msgs interpolation lanelet2_extension + libgoogle-glog-dev motion_utils rclcpp rclcpp_components diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 2ceb22fd6e7b9..f89c83e9c2d5a 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -43,6 +43,8 @@ #include #endif +#include + #include #include #include @@ -718,6 +720,8 @@ void replaceObjectYawWithLaneletsYaw( MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) { + google::InitGoogleLogging("map_based_prediction_node"); + google::InstallFailureSignalHandler(); enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); lateral_control_time_horizon_ = @@ -760,6 +764,9 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ num_continuous_state_transition_ = declare_parameter("lane_change_detection.num_continuous_state_transition"); + + consider_only_routable_neighbours_ = + declare_parameter("lane_change_detection.consider_only_routable_neighbours"); } reference_path_resolution_ = declare_parameter("reference_path_resolution"); /* prediction path will disabled when the estimated path length exceeds lanelet length. This @@ -988,6 +995,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (auto & predicted_path : predicted_paths) { predicted_path.confidence = predicted_path.confidence / sum_confidence; + if (predicted_object.kinematics.predicted_paths.size() >= 100) break; predicted_object.kinematics.predicted_paths.push_back(predicted_path); } output.objects.push_back(predicted_object); @@ -1514,19 +1522,22 @@ std::vector MapBasedPredictionNode::getPredictedReferencePath( if (!!opt) { return *opt; } - const auto adjacent = get_left ? routing_graph_ptr_->adjacentLeft(lanelet) - : routing_graph_ptr_->adjacentRight(lanelet); - if (!!adjacent) { - return *adjacent; - } - // search for unconnected lanelet - const auto unconnected_lanelets = get_left - ? getLeftLineSharingLanelets(lanelet, lanelet_map_ptr_) - : getRightLineSharingLanelets(lanelet, lanelet_map_ptr_); - // just return first candidate of unconnected lanelet for now - if (!unconnected_lanelets.empty()) { - return unconnected_lanelets.front(); + if (!consider_only_routable_neighbours_) { + const auto adjacent = get_left ? routing_graph_ptr_->adjacentLeft(lanelet) + : routing_graph_ptr_->adjacentRight(lanelet); + if (!!adjacent) { + return *adjacent; + } + // search for unconnected lanelet + const auto unconnected_lanelets = + get_left ? getLeftLineSharingLanelets(lanelet, lanelet_map_ptr_) + : getRightLineSharingLanelets(lanelet, lanelet_map_ptr_); + // just return first candidate of unconnected lanelet for now + if (!unconnected_lanelets.empty()) { + return unconnected_lanelets.front(); + } } + // if no candidate lanelet found, return empty return std::nullopt; }; diff --git a/perception/multi_object_tracker/CMakeLists.txt b/perception/multi_object_tracker/CMakeLists.txt index 7dc7ee9d75cf3..3e379bcfd1cf1 100644 --- a/perception/multi_object_tracker/CMakeLists.txt +++ b/perception/multi_object_tracker/CMakeLists.txt @@ -31,9 +31,6 @@ set(MULTI_OBJECT_TRACKER_SRC src/tracker/model/unknown_tracker.cpp src/tracker/model/pass_through_tracker.cpp src/data_association/data_association.cpp -) - -ament_auto_add_library(mu_successive_shortest_path SHARED src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) @@ -42,7 +39,6 @@ ament_auto_add_library(multi_object_tracker_node SHARED ) target_link_libraries(multi_object_tracker_node - mu_successive_shortest_path Eigen3::Eigen ) diff --git a/perception/object_merger/CMakeLists.txt b/perception/object_merger/CMakeLists.txt index 0d5ca532bdd9d..b02983db11af3 100644 --- a/perception/object_merger/CMakeLists.txt +++ b/perception/object_merger/CMakeLists.txt @@ -18,17 +18,13 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(mu_successive_shortest_path SHARED - src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp -) - ament_auto_add_library(object_association_merger SHARED src/object_association_merger/data_association/data_association.cpp + src/object_association_merger/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/object_association_merger/node.cpp ) target_link_libraries(object_association_merger - mu_successive_shortest_path Eigen3::Eigen ) diff --git a/perception/radar_object_tracker/CMakeLists.txt b/perception/radar_object_tracker/CMakeLists.txt index 3e3afacadd00f..54ef7b047bf17 100644 --- a/perception/radar_object_tracker/CMakeLists.txt +++ b/perception/radar_object_tracker/CMakeLists.txt @@ -18,11 +18,6 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -# Targets -ament_auto_add_library(mu_successive_shortest_path SHARED - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp -) - ament_auto_add_library(radar_object_tracker_node SHARED src/radar_object_tracker_node/radar_object_tracker_node.cpp src/tracker/model/tracker_base.cpp @@ -30,10 +25,10 @@ ament_auto_add_library(radar_object_tracker_node SHARED src/tracker/model/constant_turn_rate_motion_tracker.cpp src/utils/utils.cpp src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp ) target_link_libraries(radar_object_tracker_node - mu_successive_shortest_path Eigen3::Eigen yaml-cpp nlohmann_json::nlohmann_json # for debug diff --git a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp b/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp index 93d9683ba2939..3b2475286fa61 100644 --- a/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp +++ b/perception/shape_estimation/include/shape_estimation/corrector/truck_corrector.hpp @@ -26,11 +26,11 @@ class TruckCorrector : public VehicleCorrector { corrector_utils::CorrectionBBParameters params; params.min_width = 1.5; - params.max_width = 3.2; + params.max_width = 3.5; params.default_width = (params.min_width + params.max_width) * 0.5; params.min_length = 4.0; - params.max_length = 7.9; - params.default_length = (params.min_length + params.max_length) * 0.5; + params.max_length = 18.0; + params.default_length = 7.0; // 7m is the most common length of a truck in Japan setParams(params); } diff --git a/perception/shape_estimation/lib/filter/truck_filter.cpp b/perception/shape_estimation/lib/filter/truck_filter.cpp index 672082fe305fd..b7d75c6c8acdf 100644 --- a/perception/shape_estimation/lib/filter/truck_filter.cpp +++ b/perception/shape_estimation/lib/filter/truck_filter.cpp @@ -19,7 +19,7 @@ bool TruckFilter::filter( [[maybe_unused]] const geometry_msgs::msg::Pose & pose) { constexpr float min_width = 1.5; - constexpr float max_width = 3.2; - constexpr float max_length = 7.9; // upto 12m in japanese law + constexpr float max_width = 3.5; + constexpr float max_length = 18.0; // upto 12m in japanese law return utils::filterVehicleBoundingBox(shape, min_width, max_width, max_length); } diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index ed5aa76afbfd9..5e46b9403462d 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/perception/tracking_object_merger/CMakeLists.txt @@ -21,19 +21,15 @@ include_directories( ${EIGEN3_INCLUDE_DIR} ) -ament_auto_add_library(mu_successive_shortest_path SHARED - src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp -) - ament_auto_add_library(decorative_tracker_merger_node SHARED src/data_association/data_association.cpp + src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp src/decorative_tracker_merger.cpp src/utils/utils.cpp src/utils/tracker_state.cpp ) target_link_libraries(decorative_tracker_merger_node - mu_successive_shortest_path Eigen3::Eigen ) diff --git a/planning/.pages b/planning/.pages new file mode 100644 index 0000000000000..7cbd36d2ca2d0 --- /dev/null +++ b/planning/.pages @@ -0,0 +1,83 @@ +nav: + - 'Introduction': planning + - 'Behavior Path Planner': + - 'About Behavior Path': planning/behavior_path_planner + - 'Concept': + - 'Planner Manager': planning/behavior_path_planner/docs/behavior_path_planner_manager_design + - 'Scene Module Interface': planning/behavior_path_planner/docs/behavior_path_planner_interface_design + - 'Path Generation': planning/behavior_path_planner/docs/behavior_path_planner_path_generation_design + - 'Collision Assessment/Safety Check': planning/behavior_path_planner/docs/behavior_path_planner_safety_check + - 'Dynamic Drivable Area': planning/behavior_path_planner/docs/behavior_path_planner_drivable_area_design + - 'Turn Signal': planning/behavior_path_planner/docs/behavior_path_planner_turn_signal_design + - 'Scene Module': + - 'Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design + - 'Avoidance by Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design + - 'Dynamic Avoidance': planning/behavior_path_planner/docs/behavior_path_planner_dynamic_avoidance_design + - 'Goal Planner': planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design + - 'Lane Change': planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design + - 'Side Shift': planning/behavior_path_planner/docs/behavior_path_planner_side_shift_design + - 'Start Planner': planning/behavior_path_planner/docs/behavior_path_planner_start_planner_design + - 'Behavior Velocity Planner': + - 'About Behavior Velocity': planning/behavior_velocity_planner + - 'Template for Custom Module': planning/behavior_velocity_template_module + - 'Available Module': + - 'Blind Spot': planning/behavior_velocity_blind_spot_module + - 'Crosswalk': planning/behavior_velocity_crosswalk_module + - 'Detection Area': planning/behavior_velocity_detection_area_module + - 'Intersection': planning/behavior_velocity_intersection_module + - 'No Drivable Lane': planning/behavior_velocity_no_drivable_lane_module + - 'No Stopping Area': planning/behavior_velocity_no_stopping_area_module + - 'Occlusion Spot': planning/behavior_velocity_occlusion_spot_module + - 'Out of Lane': planning/behavior_velocity_out_of_lane_module + - 'Run Out': planning/behavior_velocity_run_out_module + - 'Speed Bump': planning/behavior_velocity_speed_bump_module + - 'Stop Line': planning/behavior_velocity_stop_line_module + - 'Traffic Light': planning/behavior_velocity_traffic_light_module + - 'Virtual Traffic Light': planning/behavior_velocity_virtual_traffic_light_module + - 'Walkway': planning/behavior_velocity_walkway_module + - 'Parking': + - 'Freespace Planner': + - 'About Freespace Planner': planning/freespace_planner + - 'Algorithm': planning/freespace_planning_algorithms + - 'RRT*': planning/freespace_planning_algorithms/rrtstar + - 'Mission Planner': planning/mission_planner + - 'Motion Planning': + - 'Obstacle Avoidance Planner': + - 'About Obstacle Avoidance': planning/obstacle_avoidance_planner + - 'Model Predictive Trajectory (MPT)': planning/obstacle_avoidance_planner/docs/mpt + - 'How to Debug': planning/obstacle_avoidance_planner/docs/debug + - 'Obstacle Cruise Planner': + - 'About Obstacle Cruise': planning/obstacle_cruise_planner + - 'How to Debug': planning/obstacle_cruise_planner/docs/debug + - 'Obstacle Stop Planner': planning/obstacle_stop_planner + - 'Obstacle Velocity Limiter': planning/obstacle_velocity_limiter + - 'Path Smoother': + - 'About Path Smoother': planning/path_smoother + - 'Elastic Band': planning/path_smoother/docs/eb + - 'Sampling Based Planner': + - 'Path Sample': planning/sampling_based_planner/path_sampler + - 'Common library': planning/sampling_based_planner/sampler_common + - 'Frenet Planner': planning/sampling_based_planner/frenet_planner + - 'Bezier Sampler': planning/sampling_based_planner/bezier_sampler + - 'Surround Obstacle Checker': + - 'About Surround Obstacle Checker': planning/surround_obstacle_checker + - 'About Surround Obstacle Checker (Japanese)': planning/surround_obstacle_checker/surround_obstacle_checker-design.ja + - 'Motion Velocity Smoother': + - 'About Motion Velocity Smoother': planning/motion_velocity_smoother + - 'About Motion Velocity Smoother (Japanese)': planning/motion_velocity_smoother/README.ja + - 'Scenario Selector': planning/scenario_selector + - 'Static Centerline Optimizer': planning/static_centerline_optimizer + - 'API and Library': + - 'Costmap Generator': planning/costmap_generator + - 'External Velocity Limit Selector': planning/external_velocity_limit_selector + - 'Objects of Interest Marker Interface': planning/objects_of_interest_marker_interface + - 'Route Handler': planning/route_handler + - 'RTC Interface': planning/rtc_interface + - 'Additional Tools': + - 'RTC Replayer': planning/rtc_replayer + - 'Planning Debug Tools': + - 'About Planning Debug Tools': planning/planning_debug_tools + - 'Stop Reason Visualizer': planning/planning_debug_tools/doc-stop-reason-visualizer + - 'Planning Test Utils': planning/planning_test_utils + - 'Planning Topic Converter': planning/planning_topic_converter + - 'Planning Validator': planning/planning_validator diff --git a/planning/README.md b/planning/README.md new file mode 100644 index 0000000000000..4f13bac3392a0 --- /dev/null +++ b/planning/README.md @@ -0,0 +1,122 @@ +# Planning Components + +## Getting Started + +The Autoware.Universe Planning Modules represent a cutting-edge component within the broader open-source autonomous driving software stack. These modules play a pivotal role in autonomous vehicle navigation, skillfully handling route planning, dynamic obstacle avoidance, and real-time adaptation to varied traffic conditions. + +- For high level concept of Planning Components, please refer to [Planning Component Design Document](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) +- To understand how Planning Components interacts with other components, please refer to [Planning Component Interface Document](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/planning/) +- The [Node Diagram](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/node-diagram/) illustrates the interactions, inputs, and outputs of all modules in the Autoware.Universe, including planning modules. + +## Planning Module + +The **Module** in the Planning Component refers to the various components that collectively form the planning system of the software. These modules cover a range of functionalities necessary for autonomous vehicle planning. Autoware's planning modules are modularized, meaning users can customize which functions are enabled by changing the configuration. This modular design allows for flexibility and adaptability to different scenarios and requirements in autonomous vehicle operations. + +### How to Enable or Disable Planning Module + +Enabling and disabling modules involves managing settings in key configuration and launch files. + +### Key Files for Configuration + +The `default_preset.yaml` file acts as the primary configuration file, where planning modules can be disable or enabled. Furthermore, users can also set the type of motion planner across various motion planners. For example: + +- `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. +- `motion_stop_planner_type`: Set `default` to either `obstacle_stop_planner` or `obstacle_cruise_planner`. + +!!! note + + Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. + +The [launch files](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving) reference the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `avoidance.enable_module` in + +```xml + +``` + +corresponds to launch_avoidance_module from `default_preset.yaml`. + +### Parameters configuration + +There are multiple parameters available for configuration, and users have the option to modify them in [here](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning). It's important to note that not all parameters are adjustable via `rqt_reconfigure`. To ensure the changes are effective, modify the parameters and then restart Autoware. Additionally, detailed information about each parameter is available in the corresponding documents under the planning tab. + +### Integrating a Custom Module into Autoware: A Step-by-Step Guide + +This guide outlines the steps for integrating your custom module into Autoware: + +- Add your modules to the `default_present.yaml` file. For example + +```yaml +- arg: + name: launch_intersection_module + default: "true" +``` + +- Incorporate your modules into the [launcher](https://github.com/autowarefoundation/autoware.universe/tree/main/launch/tier4_planning_launch/launch/scenario_planning). For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/main/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml): + +```xml + + + +``` + +- If applicable, place your parameter folder within the appropriate existing parameter folder. For example [intersection_module's parameters](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml) is in [behavior_velocity_planner](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner). +- Insert the path of your parameters in the [tier4_planning_component.launch.xml](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/launch/components/tier4_planning_component.launch.xml). For example `behavior_velocity_planner_intersection_module_param_path` is used. + +```xml + +``` + +- Define your parameter path variable within the corresponding launcher. For example in [behavior_planning.launch.xml](https://github.com/autowarefoundation/autoware.universe/blob/04aa54bf5fb0c88e70198ca74b9ac343cc3457bf/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml#L191) + +```xml + +``` + +!!! note + + Depending on the specific module you wish to add, the relevant files and steps may vary. This guide provides a general overview and serves as a starting point. It's important to adapt these instructions to the specifics of your module. + +## Join Our Community-Driven Effort + +Autoware thrives on community collaboration. Every contribution, big or small, is invaluable to us. Whether it's reporting bugs, suggesting improvements, offering new ideas, or anything else you can think of – we welcome it all with open arms. + +### How to Contribute? + +Ready to contribute? Great! To get started, simply visit our [Contributing Guidelines](https://autowarefoundation.github.io/autoware-documentation/main/contributing/) where you'll find all the information you need to jump in. This includes instructions on submitting bug reports, proposing feature enhancements, and even contributing to the codebase. + +### Join Our Planning & Control Working Group Meetings + +The Planning & Control working group is an integral part of our community. We meet bi-weekly to discuss our current progress, upcoming challenges, and brainstorm new ideas. These meetings are a fantastic opportunity to directly contribute to our discussions and decision-making processes. + +Meeting Details: + +- **Frequency:** Bi-weekly +- **Day:** Thursday +- **Time:** 08:00 AM UTC (05:00 PM JST) +- **Agenda:** Discuss current progress, plan future developments. You can view and comment on the minutes of past meetings [here](https://github.com/orgs/autowarefoundation/discussions?discussions_q=is%3Aopen+label%3Ameeting%3Aplanning-control-wg+). + +Interested in joining our meetings? We’d love to have you! For more information on how to participate, visit the following link: [How to participate in the working group](https://github.com/autowarefoundation/autoware-projects/wiki/Autoware-Planning-Control-Working-Group#how-to-participate-in-the-working-group). + +### Citations + +Occasionally, we publish papers specific to the Planning Component in Autoware. We encourage you to explore these publications and find valuable insights for your work. If you find them useful and incorporate any of our methodologies or algorithms in your projects, citing our papers would be immensely helpful. This support allows us to reach a broader audience and continue contributing to the field. + +If you use the Jerk Constrained Velocity Planning algorithm in [Motion Velocity Smoother](./motion_velocity_smoother/README.md) module in the Planning Component, we kindly request you to cite the relevant paper. + + + +Y. Shimizu, T. Horibe, F. Watanabe and S. Kato, "[Jerk Constrained Velocity Planning for an Autonomous Vehicle: Linear Programming Approach](https://arxiv.org/abs/2202.10029)," 2022 International Conference on Robotics and Automation (ICRA) + +```tex +@inproceedings{shimizu2022, + author={Shimizu, Yutaka and Horibe, Takamasa and Watanabe, Fumiya and Kato, Shinpei}, + booktitle={2022 International Conference on Robotics and Automation (ICRA)}, + title={Jerk Constrained Velocity Planning for an Autonomous Vehicle: Linear Programming Approach}, + year={2022}, + pages={5814-5820}, + doi={10.1109/ICRA46639.2022.9812155}} +``` diff --git a/planning/behavior_path_planner/CMakeLists.txt b/planning/behavior_path_planner/CMakeLists.txt index 7fc35355202ff..2ea43bf5a4ef1 100644 --- a/planning/behavior_path_planner/CMakeLists.txt +++ b/planning/behavior_path_planner/CMakeLists.txt @@ -7,11 +7,11 @@ autoware_package() find_package(OpenCV REQUIRED) find_package(magic_enum CONFIG REQUIRED) -ament_auto_add_library(behavior_path_planner_node SHARED +pluginlib_export_plugin_description_file(${PROJECT_NAME} plugins.xml) + +ament_auto_add_library(${PROJECT_NAME}_lib SHARED src/planner_manager.cpp - src/steering_factor_interface.cpp src/behavior_path_planner_node.cpp - src/scene_module/scene_module_visitor.cpp src/scene_module/avoidance/avoidance_module.cpp src/scene_module/avoidance/manager.cpp src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -27,13 +27,8 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/scene_module/lane_change/external_request.cpp src/scene_module/lane_change/avoidance_by_lane_change.cpp src/scene_module/lane_change/manager.cpp - src/turn_signal_decider.cpp - src/utils/utils.cpp - src/utils/path_utils.cpp - src/utils/traffic_light_utils.cpp - src/utils/path_safety_checker/safety_check.cpp - src/utils/path_safety_checker/objects_filtering.cpp src/utils/start_goal_planner_common/utils.cpp + src/utils/avoidance/shift_line_generator.cpp src/utils/avoidance/utils.cpp src/utils/lane_change/utils.cpp src/utils/side_shift/util.cpp @@ -47,39 +42,33 @@ ament_auto_add_library(behavior_path_planner_node SHARED src/utils/start_planner/shift_pull_out.cpp src/utils/start_planner/geometric_pull_out.cpp src/utils/start_planner/freespace_pull_out.cpp - src/utils/path_shifter/path_shifter.cpp - src/utils/drivable_area_expansion/static_drivable_area.cpp - src/utils/drivable_area_expansion/drivable_area_expansion.cpp - src/utils/drivable_area_expansion/map_utils.cpp - src/utils/drivable_area_expansion/footprints.cpp src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp src/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.cpp - src/marker_utils/utils.cpp src/marker_utils/avoidance/debug.cpp src/marker_utils/lane_change/debug.cpp ) -target_include_directories(behavior_path_planner_node SYSTEM PUBLIC +target_include_directories(${PROJECT_NAME}_lib SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR} ) -target_link_libraries(behavior_path_planner_node +target_link_libraries(${PROJECT_NAME}_lib ${OpenCV_LIBRARIES} ) -rclcpp_components_register_node(behavior_path_planner_node +rclcpp_components_register_node(${PROJECT_NAME}_lib PLUGIN "behavior_path_planner::BehaviorPathPlannerNode" - EXECUTABLE behavior_path_planner + EXECUTABLE ${PROJECT_NAME}_node ) if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_utilities test/input.cpp test/test_utils.cpp - test/test_drivable_area_expansion.cpp ) + target_link_libraries(test_${CMAKE_PROJECT_NAME}_utilities - behavior_path_planner_node + ${PROJECT_NAME}_lib ) ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_avoidance_module @@ -87,7 +76,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_${CMAKE_PROJECT_NAME}_avoidance_module - behavior_path_planner_node + ${PROJECT_NAME}_lib ) ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_lane_change_module @@ -95,30 +84,15 @@ if(BUILD_TESTING) ) target_link_libraries(test_${CMAKE_PROJECT_NAME}_lane_change_module - behavior_path_planner_node - ) - - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_safety_check - test/test_safety_check.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_safety_check - behavior_path_planner_node - ) - - ament_add_ros_isolated_gmock(test_${CMAKE_PROJECT_NAME}_turn_signal - test/test_turn_signal.cpp - ) - - target_link_libraries(test_${CMAKE_PROJECT_NAME}_turn_signal - behavior_path_planner_node + ${PROJECT_NAME}_lib ) ament_add_ros_isolated_gtest(test_${PROJECT_NAME}_node_interface test/test_${PROJECT_NAME}_node_interface.cpp ) + target_link_libraries(test_${PROJECT_NAME}_node_interface - behavior_path_planner_node + ${PROJECT_NAME}_lib ) endif() diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 0c0c7c806c119..884767315ac12 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -1,190 +1,284 @@ # Behavior Path Planner -## Purpose / Use cases +The Behavior Path Planner's main objective is to significantly enhance the safety of autonomous vehicles by minimizing the risk of accidents. It improves driving efficiency through time conservation and underpins reliability with its rule-based approach. Additionally, it allows users to integrate their own custom behavior modules or use it with different types of vehicles, such as cars, buses, and delivery robots, as well as in various environments, from busy urban streets to open highways. -The `behavior_path_planner` module is responsible to generate +The module begins by thoroughly analyzing the ego vehicle's current situation, including its position, speed, and surrounding environment. This analysis leads to essential driving decisions about lane changes or stopping and subsequently generates a path that is both safe and efficient. It considers road geometry, traffic rules, and dynamic conditions while also incorporating obstacle avoidance to respond to static and dynamic obstacles such as other vehicles, pedestrians, or unexpected roadblocks, ensuring safe navigation. -1. **path** based on the traffic situation, -2. **drivable area** that the vehicle can move (defined in the path msg), -3. **turn signal** command to be sent to the vehicle interface. +Moreover, the planner actively interacts with other traffic participants, predicting their actions and accordingly adjusting the vehicle's path. This ensures not only the safety of the autonomous vehicle but also contributes to smooth traffic flow. Its adherence to traffic laws, including speed limits and traffic signals, further guarantees lawful and predictable driving behavior. The planner is also designed to minimize sudden or abrupt maneuvers, aiming for a comfortable and natural driving experience. -## Design +!!! note -### Scene modules design + The [Planning Component Design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/) Document outlines the foundational philosophy guiding the design and future development of the Behavior Path Planner module. We strongly encourage readers to consult this document to understand the rationale behind its current configuration and the direction of its ongoing development. -#### Support modules +## Purpose / Use Cases -Behavior path planner has following scene modules. +Essentially, the module has three primary responsibilities: -| Name | Description | Details | -| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | -| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_by_lane_change_design.md) | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | -| Pull Out | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | +1. Creating a **path based** on the traffic situation. +2. Generating **drivable area**, i.e. the area within which the vehicle can maneuver. +3. Generating **turn signal** commands to be relayed to the vehicle interface. -![behavior_modules](./image/behavior_modules.png) +## Features -All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. +### Supported Scene Modules -#### How to implement new module? +Behavior Path Planner has following scene modules -WIP +| Name | Description | Details | +| :----------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | +| Dynamic Avoidance | WIP | LINK | +| Avoidance By Lane Change | this module generates lane change path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_by_lane_change_design.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | +| External Lane Change | WIP | LINK | +| Start Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | +| Goal Planner | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | ---- +!!! Note -### Manager design + click on the following images to view the video of their execution -The role of manager is to launch the appropriate scene module according to the situation. (e.g. if there is parked-vehicle in ego's driving lane, the manager launches the avoidance module.) +
+ + + + + + + + + + + +
Lane Following ModuleAvoidance ModuleAvoidance by Lane Change Module
Lane Change ModuleStart Planner ModuleGoal Planner Module
+
-Now, it is able to select two managers with different architecture. +!!! Note -| Name | Description | Details | -| :------------------------------------ | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------- | -| Behavior Tree based manager (default) | this manager launches scene modules based on Behavior Tree. all scene modules run exclusively. | LINK(WIP) | -| BT-free manager (unstable) | this manager is developed in order to achieve complex scenario, and launches scene modules without Behavior Tree. multiple modules can run simultaneously on this manager. | [LINK](./docs/behavior_path_planner_manager_design.md) | + Users can refer to [Planning component design](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-architecture/planning/#supported-functions) for some additional behavior. -The manager is switched by flag `COMPILE_WITH_OLD_ARCHITECTURE` in CmakeLists.txt of `behavior_path_planner` package. Please set the flag **FALSE** if you try to use BT-free manager. +#### How to add or implement new module? -```cmake -cmake_minimum_required(VERSION 3.14) -project(behavior_path_planner) +All scene modules are implemented by inheriting base class `scene_module_interface.hpp`. -find_package(autoware_cmake REQUIRED) -autoware_package() +!!! Warning -find_package(OpenCV REQUIRED) -find_package(magic_enum CONFIG REQUIRED) + The remainder of this subsection is work in progress (WIP). -set(COMPILE_WITH_OLD_ARCHITECTURE TRUE) # <- HERE +### Planner Manager -... -``` +The Planner Manager's responsibilities include: -**What is the Behavior Tree?**: In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine. +1. Activating the relevant scene module in response to the specific situation faced by the autonomous vehicle. For example, when a parked vehicle blocks the ego vehicle's driving lane, the manager would engage the avoidance module. +2. Managing the execution order when multiple modules are running simultaneously. For instance, if both the lane-changing and avoidance modules are operational, the manager decides which should take precedence. +3. Merging paths from multiple modules when they are activated simultaneously and each generates its own path, thereby creating a single functional path. -The current behavior tree structure is shown below. Each modules (LaneChange, Avoidance, etc) have _Request_, _Ready_, and _Plan_ nodes as a common function. +!!! note -- **Request**: Check if there is a request from the module (e.g. LaneChange has a request when there are multi-lanes and the vehicle is not on the preferred lane), -- **Ready**: Check if it is safe to execute the plan (e.g. LaneChange is ready when the lane_change path does not have any conflicts with other dynamic objects on S-T space). -- **Plan**: Calculates path and set it to the output of the BehaviorTree. Until the internal status returns SUCCESS, it will be in running state and will not transit to another module. -- **ForceApproval**: A lane change-specific node that overrides the result of _Ready_ when a forced lane change command is given externally. + To check the scene module's transition, i.e.: registered, approved and candidate modules, set `verbose: true` in the [behavior path planner configuration file](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml#L3). -![behavior_path_planner_bt_config](./image/behavior_path_planner_bt_config.png) + ![Scene module's transition table](./image/checking_module_transition.png) + +!!! note + + For more in-depth information, refer to [Manager design](./docs/behavior_path_planner_manager_design.md) document. ## Inputs / Outputs / API -### output +### Input -| Name | Type | Description | -| :-------------------------- | :------------------------------------------------------- | :--------------------------------------------------------------------------------------------- | -| ~/input/path | `autoware_auto_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | -| ~/input/path_candidate | `autoware_auto_planning_msgs::msg::Path` | the path the module is about to take. to be executed as soon as external approval is obtained. | -| ~/input/turn_indicators_cmd | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | -| ~/input/hazard_lights_cmd | `tier4_planning_msgs::msg::PathChangeModuleArray` | hazard lights command. | -| ~/input/ready_module | `tier4_planning_msgs::msg::PathChangeModule` | (for remote control) modules that are ready to be executed. | -| ~/input/running_modules | `tier4_planning_msgs::msg::PathChangeModuleArray` | (for remote control) current running module. | +| Name | Required? | Type | Description | +| :---------------------------- | :-------: | :----------------------------------------------------- | :----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| ~/input/odometry | ○ | `nav_msgs::msg::Odometry` | for ego velocity. | +| ~/input/accel | ○ | `geometry_msgs::msg::AccelWithCovarianceStamped` | for ego acceleration. | +| ~/input/objects | ○ | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | +| ~/input/occupancy_grid_map | ○ | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | +| ~/input/traffic_signals | ○ | `autoware_perception_msgs::msg::TrafficSignalArray` | traffic signals information from the perception module | +| ~/input/vector_map | ○ | `autoware_auto_mapping_msgs::msg::HADMapBin` | vector map information. | +| ~/input/route | ○ | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | +| ~/input/scenario | ○ | `tier4_planning_msgs::msg::Scenario` | Launches behavior path planner if current scenario == `Scenario:LaneDriving`. | +| ~/input/lateral_offset | △ | `tier4_planning_msgs::msg::LateralOffset` | lateral offset to trigger side shift | +| ~/system/operation_mode/state | ○ | `autoware_adapi_v1_msgs::msg::OperationModeState` | Allows planning module to know if vehicle is in autonomous mode or can be controlled[ref](https://github.com/autowarefoundation/autoware.universe/blob/main/system/default_ad_api/document/operation-mode.md) | -### input +- ○ Mandatory: Planning Module would not work if anyone of this is not present. +- △ Optional: Some module would not work, but Planning Module can still be operated. -| Name | Type | Description | -| :----------------------------- | :----------------------------------------------------- | :------------------------------------------------------------------------------------ | -| ~/input/route | `autoware_auto_mapping_msgs::msg::LaneletRoute` | current route from start to goal. | -| ~/input/vector_map | `autoware_auto_mapping_msgs::msg::HADMapBin` | map information. | -| ~/input/objects | `autoware_auto_perception_msgs::msg::PredictedObjects` | dynamic objects from perception module. | -| ~/input/occupancy_grid_map/map | `nav_msgs::msg::OccupancyGrid` | occupancy grid map from perception module. This is used for only Goal Planner module. | -| ~/input/kinematic_state | `nav_msgs::msg::Odometry` | for ego velocity. | +### Output -## General features of behavior path planner +| Name | Type | Description | QoS Durability | +| :---------------------------- | :------------------------------------------------------- | :--------------------------------------------------------------------------------------------- | ----------------- | +| ~/output/path | `autoware_auto_planning_msgs::msg::PathWithLaneId` | the path generated by modules. | `volatile` | +| ~/output/turn_indicators_cmd | `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` | turn indicators command. | `volatile` | +| ~/output/hazard_lights_cmd | `autoware_auto_vehicle_msgs::msg::HazardLightsCommand` | hazard lights command. | `volatile` | +| ~/output/modified_goal | `autoware_planning_msgs::msg::PoseWithUuidStamped` | output modified goal commands. | `transient_local` | +| ~/output/stop_reasons | `tier4_planning_msgs::msg::StopReasonArray` | describe the reason for ego vehicle stop | `volatile` | +| ~/output/reroute_availability | `tier4_planning_msgs::msg::RerouteAvailability` | the path the module is about to take. to be executed as soon as external approval is obtained. | `volatile` | -### Drivable area generation logic +### Debug -The behavior path planner generates drivable area that is defined in `autoware_auto_planning_msgs::msg::PathWithLaneId` and `autoware_auto_planning_msgs::msg::Path` messages as: +| Name | Type | Description | QoS Durability | +| :-------------------------------------- | :-------------------------------------------------- | :---------------------------------------------------------------------------------------- | -------------- | +| ~/debug/avoidance_debug_message_array | `tier4_planning_msgs::msg::AvoidanceDebugMsgArray` | debug message for avoidance. notify users reasons for avoidance path cannot be generated. | `volatile` | +| ~/debug/lane_change_debug_message_array | `tier4_planning_msgs::msg::LaneChangeDebugMsgArray` | debug message for lane change. notify users unsafe reason during lane changing process | `volatile` | +| ~/debug/maximum_drivable_area | `visualization_msgs::msg::MarkerArray` | shows maximum static drivable area. | `volatile` | +| ~/debug/turn_signal_info | `visualization_msgs::msg::MarkerArray` | TBA | `volatile` | +| ~/debug/bound | `visualization_msgs::msg::MarkerArray` | debug for static drivable area | `volatile` | +| ~/planning/path_candidate/\* | `autoware_auto_planning_msgs::msg::Path` | the path before approval. | `volatile` | +| ~/planning/path_reference/\* | `autoware_auto_planning_msgs::msg::Path` | reference path generated by each modules. | `volatile` | -```c++ -std::vector left_bound; -std::vector right_bound; -``` +!!! note -Optionally, the drivable area can be expanded by a static distance. -Expansion parameters are defined for each module of the `behavior_path_planner` and should be prefixed accordingly (see `config/drivable_area_expansion.yaml` for an example). + For specific information of which topics are being subscribed and published, refer to [behavior_path_planner.xml](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml#L36-L49). -| Name | Unit | Type | Description | Default value | -| :------------------------------- | :--- | :-------------- | :----------------------------------------- | :------------ | -| drivable_area_right_bound_offset | [m] | double | expansion distance of the right bound | 0.0 | -| drivable_area_right_bound_offset | [m] | double | expansion distance of the left bound | 0.0 | -| drivable_area_types_to_skip | | list of strings | types of linestrings that are not expanded | [road_border] | +## How to enable or disable the modules -Click [here](./docs/behavior_path_planner_drivable_area_design.md) for details. +Enabling and disabling the modules in the behavior path planner is primarily managed through two key files: `default_preset.yaml` and `behavior_path_planner.launch.xml`. -### Turn signal decision logic +The `default_preset.yaml` file acts as a configuration file for enabling or disabling specific modules within the planner. It contains a series of arguments which represent the behavior path planner's modules or features. For example: -The behavior path planner outputs turn signal information as `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand`. It uses rule-based algorithm to determine blinkers. +- `launch_avoidance_module`: Set to `true` to enable the avoidance module, or `false` to disable it. +- `use_experimental_lane_change_function`: Set to `true` to enable experimental features in the lane change module. -```c++ -module autoware_auto_vehicle_msgs { - module msg { - module TurnIndicatorsCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 DISABLE = 1; - const uint8 ENABLE_LEFT = 2; - const uint8 ENABLE_RIGHT = 3; - }; +!!! note - @verbatim (language="comment", text= - " Command for controlling turn indicators.") + Click [here](https://github.com/autowarefoundation/autoware_launch/blob/main/autoware_launch/config/planning/preset/default_preset.yaml) to view the `default_preset.yaml`. - struct TurnIndicatorsCommand { - builtin_interfaces::msg::Time stamp; +The `behavior_path_planner.launch.xml` file references the settings defined in `default_preset.yaml` to apply the configurations when the behavior path planner's node is running. For instance, the parameter `avoidance.enable_module` in - @default (value=0) - uint8 command; - }; - }; -}; +```xml + ``` -Click [here](./docs/behavior_path_planner_turn_signal_design.md) for details. +corresponds to launch_avoidance_module from `default_preset.yaml`. -### Shifted path generation logic +Therefore, to enable or disable a module, simply set the corresponding module in `default_preset.yaml` to `true` or `false`. These changes will be applied upon the next launch of Autoware. -Some modules have to generate shifted path from reference path, then shift path generation logic is implemented as library **path shifter**. **path shifter** takes a reference path and shift lines and output a shifted path. +## Generating Path -```c++ -struct ShiftLine -{ - Pose start{}; // shift start point in absolute coordinate - Pose end{}; // shift start point in absolute coordinate +A sophisticated methodology is used for path generation, particularly focusing on maneuvers like lane changes and avoidance. At the core of this design is the smooth lateral shifting of the reference path, achieved through a constant-jerk profile. This approach ensures a consistent rate of change in acceleration, facilitating smooth transitions and minimizing abrupt changes in lateral dynamics, crucial for passenger comfort and safety. - // relative shift length at the start point related to the reference path - double start_shift_length{}; +The design involves complex mathematical formulations for calculating the lateral shift of the vehicle's path over time. These calculations include determining lateral displacement, velocity, and acceleration, while considering the vehicle's lateral acceleration and velocity limits. This is essential for ensuring that the vehicle's movements remain safe and manageable. - // relative shift length at the end point related to the reference path - double end_shift_length{}; +The `ShiftLine` struct (as seen [here](https://github.com/autowarefoundation/autoware.universe/blob/9000f430c937764c14e43109539302f1f878ed70/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp#L35-L48)) is utilized to represent points along the path where the lateral shift starts and ends. It includes details like the start and end points in absolute coordinates, the relative shift lengths at these points compared to the reference path, and the associated indexes on the reference path. This struct is integral to managing the path shifts, as it allows the path planner to dynamically adjust the trajectory based on the vehicle's current position and planned maneuver. - size_t start_idx{}; // associated start-point index for the reference path - size_t end_idx{}; // associated end-point index for the reference path -}; -``` +Furthermore, the design and its implementation incorporate various equations and mathematical models to calculate essential parameters for the path shift. These include the total distance of the lateral shift, the maximum allowable lateral acceleration and jerk, and the total time required for the shift. Practical considerations are also noted, such as simplifying assumptions in the absence of a specific time interval for most lane change and avoidance cases. + +The shifted path generation logic enables the behavior path planner to dynamically generate safe and efficient paths, precisely controlling the vehicle’s lateral movements to ensure the smooth execution of lane changes and avoidance maneuvers. This careful planning and execution adhere to the vehicle's dynamic capabilities and safety constraints, maximizing efficiency and safety in autonomous vehicle navigation. + +!!! note + + If you're a math lover, refer to [Path Generation Design](./docs/behavior_path_planner_path_generation_design.md) for the nitty-gritty. + +## Collision Assessment / Safety check + +The purpose of the collision assessment function in the Behavior Path Planner is to evaluate the potential for collisions with target objects across all modules. It is utilized in two scenarios: + +1. During candidate path generation, to ensure that the generated candidate path is collision-free. +2. When the path is approved by the manager, and the ego vehicle is executing the current module. If the current situation is deemed unsafe, depending on each module's requirements, the planner will either cancel the execution or opt to execute another module. + +The safety check process involves several steps. Initially, it obtains the pose of the target object at a specific time, typically through interpolation of the predicted path. It then checks for any overlap between the ego vehicle and the target object at this time. If an overlap is detected, the path is deemed unsafe. The function also identifies which vehicle is in front by using the arc length along the given path. The function operates under the assumption that accurate data on the position, velocity, and shape of both the ego vehicle (the autonomous vehicle) and any target objects are available. It also relies on the yaw angle of each point in the predicted paths of these objects, which is expected to point towards the next path point. + +A critical part of the safety check is the calculation of the RSS (Responsibility-Sensitive Safety) distance-inspired algorithm. This algorithm considers factors such as reaction time, safety time margin, and the velocities and decelerations of both vehicles. Extended object polygons are created for both the ego and target vehicles. Notably, the rear object’s polygon is extended by the RSS distance longitudinally and by a lateral margin. The function finally checks for overlap between this extended rear object polygon and the front object polygon. Any overlap indicates a potential unsafe situation. + +However, the module does have a limitation concerning the yaw angle of each point in the predicted paths of target objects, which may not always accurately point to the next point, leading to potential inaccuracies in some edge cases. + +!!! note + + For further reading on the collision assessment method, please refer to [Safety check utils](./docs/behavior_path_planner_safety_check.md) + +## Generating Drivable Area + +### Static Drivable Area logic + +The drivable area is used to determine the area in which the ego vehicle can travel. The primary goal of static drivable area expansion is to ensure safe travel by generating an area that encompasses only the necessary spaces for the vehicle's current behavior, while excluding non-essential areas. For example, while `avoidance` module is running, the drivable area includes additional space needed for maneuvers around obstacles, and it limits the behavior by not extending the avoidance path outside of lanelet areas. -Click [here](./docs/behavior_path_planner_path_generation_design.md) for details. +
+ + + + + + + +
Before expansion
After expansion
+
-## References / External links +Static drivable area expansion operates under assumptions about the correct arrangement of lanes and the coverage of both the front and rear of the vehicle within the left and right boundaries. Key parameters for drivable area generation include extra footprint offsets for the ego vehicle, the handling of dynamic objects, maximum expansion distance, and specific methods for expansion. Additionally, since each module generates its own drivable area, before passing it as the input to generate the next running module's drivable area, or before generating a unified drivable area, the system sorts drivable lanes based on the vehicle's passage order. This ensures the correct definition of the lanes used in drivable area generation. -This module depends on the external [BehaviorTreeCpp](https://github.com/BehaviorTree/BehaviorTree.CPP) library. +!!! note - + Further details can is provided in [Drivable Area Design](./docs/behavior_path_planner_drivable_area_design.md). -[[1]](https://www.sciencedirect.com/science/article/pii/S1474667015347431) H. Vorobieva, S. Glaser, N. Minoiu-Enache, and S. Mammar, “Geometric path planning for automatic parallel parking in tiny spots”, IFAC Proceedings Volumes, vol. 45, no. 24, pp. 36–42, 2012. +### Dynamic Drivable Area Logic -## Future extensions / Unimplemented parts +Large vehicles require much more space, which sometimes causes them to veer out of their current lane. A typical example being a bus making a turn at a corner. In such cases, relying on a static drivable area is insufficient, since the static method depends on lane information provided by high-definition maps. To overcome the limitations of the static approach, the dynamic drivable area expansion algorithm adjusts the navigable space for an autonomous vehicle in real-time. It conserves computational power by reusing previously calculated path data, updating only when there is a significant change in the vehicle's position. The system evaluates the minimum lane width necessary to accommodate the vehicle's turning radius and other dynamic factors. It then calculates the optimal expansion of the drivable area's boundaries to ensure there is adequate space for safe maneuvering, taking into account the vehicle's path curvature. The rate at which these boundaries can expand or contract is moderated to maintain stability in the vehicle's navigation. The algorithm aims to maximize the drivable space while avoiding fixed obstacles and adhering to legal driving limits. Finally, it applies these boundary adjustments and smooths out the path curvature calculations to ensure a safe and legally compliant navigable path is maintained throughout the vehicle's operation. -- +!!! note + + The feature can be enabled in the [drivable_area_expansion.param.yaml](https://github.com/autowarefoundation/autoware_launch/blob/0cd5d891a36ac34a32a417205905c109f2bafe7b/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml#L10). + +## Generating Turn Signal + +The Behavior Path Planner module uses the `autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand` to output turn signal commands (see [TurnIndicatorsCommand.idl](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl)). The system evaluates the driving context and determines when to activate turn signals based on its maneuver planning—like turning, lane changing, or obstacle avoidance. + +Within this framework, the system differentiates between **desired** and **required** blinker activations. **Desired** activations are those recommended by traffic laws for typical driving scenarios, such as signaling before a lane change or turn. **Required** activations are those that are deemed mandatory for safety reasons, like signaling an abrupt lane change to avoid an obstacle. + +The `TurnIndicatorsCommand` message structure has a command field that can take one of several constants: `NO_COMMAND` indicates no signal is necessary, `DISABLE` to deactivate signals, `ENABLE_LEFT` to signal a left turn, and `ENABLE_RIGHT` to signal a right turn. The Behavior Path Planner sends these commands at the appropriate times, based on its rules-based system that considers both the **desired** and **required** scenarios for blinker activation. + +!!! note + + For more in-depth information, refer to [Turn Signal Design](./docs/behavior_path_planner_turn_signal_design.md) document. + +## Rerouting + +!!! warning + + Rerouting is a feature that was still under progress. Further information will be included on a later date. + +## Parameters and Configuration + +The [configuration files](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner) are organized in a hierarchical directory structure for ease of navigation and management. Each subdirectory contains specific configuration files relevant to its module. The root directory holds general configuration files that apply to the overall behavior of the planner. The following is an overview of the directory structure with the respective configuration files. + +```text +behavior_path_planner +├── behavior_path_planner.param.yaml +├── drivable_area_expansion.param.yaml +├── scene_module_manager.param.yaml +├── avoidance +│ └── avoidance.param.yaml +├── avoidance_by_lc +│ └── avoidance_by_lc.param.yaml +├── dynamic_avoidance +│ └── dynamic_avoidance.param.yaml +├── goal_planner +│ └── goal_planner.param.yaml +├── lane_change +│ └── lane_change.param.yaml +├── side_shift +│ └── side_shift.param.yaml +└── start_planner + └── start_planner.param.yaml +``` + +Similarly, the [common](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/scenario_planning/common) directory contains configuration files that are used across various modules, providing shared parameters and settings essential for the functioning of the Behavior Path Planner: + +```text +common +├── common.param.yaml +├── costmap_generator.param.yaml +└── nearest_search.param.yaml +``` + +The [preset](https://github.com/autowarefoundation/autoware_launch/tree/main/autoware_launch/config/planning/preset) directory contains the configurations for managing the operational state of various modules. It includes the default_preset.yaml file, which specifically caters to enabling and disabling modules within the system. + +```text +preset +└── default_preset.yaml +``` -## Related issues +## Limitations & Future Work -- +1. Goal Planner module cannot be simultaneously executed together with other modules. +2. Module is not designed as plugin. Integrating custom module is not straightforward and user have to modify some part of the behavior path planner main code. diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index ad06a1a9f6a10..c68c10f2a9489 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -11,14 +11,14 @@ enable_bound_clipping: false enable_yield_maneuver: true enable_yield_maneuver_during_shifting: false - enable_cancel_maneuver: false + enable_cancel_maneuver: true disable_path_update: false # drivable area setting use_adjacent_lane: true use_opposite_lane: true - use_intersection_areas: false - use_hatched_road_markings: false + use_intersection_areas: true + use_hatched_road_markings: true # for debug publish_debug_marker: false @@ -27,7 +27,6 @@ # avoidance is performed for the object type with true target_object: car: - is_target: true # [-] execute_num: 1 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] @@ -38,9 +37,8 @@ safety_buffer_longitudinal: 0.0 # [m] use_conservative_buffer_longitudinal: true # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance. truck: - is_target: true execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 @@ -49,9 +47,8 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bus: - is_target: true execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 @@ -60,9 +57,8 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true trailer: - is_target: true execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 @@ -71,9 +67,8 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true unknown: - is_target: false execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.3 @@ -82,9 +77,8 @@ safety_buffer_longitudinal: 0.0 use_conservative_buffer_longitudinal: true bicycle: - is_target: false execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 @@ -93,9 +87,8 @@ safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true motorcycle: - is_target: false execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 @@ -104,9 +97,8 @@ safety_buffer_longitudinal: 1.0 use_conservative_buffer_longitudinal: true pedestrian: - is_target: false execute_num: 1 - moving_speed_threshold: 1.0 + moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 max_expand_ratio: 0.0 envelope_buffer_margin: 0.8 @@ -119,6 +111,16 @@ # For target object filtering target_filtering: + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # detection range object_check_goal_distance: 20.0 # [m] # filtering parking objects @@ -130,7 +132,7 @@ # detection area generation parameters detection_area: - static: true # [-] + static: false # [-] min_forward_distance: 50.0 # [m] max_forward_distance: 150.0 # [m] backward_distance: 10.0 # [m] @@ -146,8 +148,22 @@ front_distance: 30.0 # [m] behind_distance: 30.0 # [m] + # params for filtering objects that are in intersection + intersection: + yaw_deviation: 0.349 # [rad] (default 20.0deg) + # For safety check safety_check: + # safety check target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: false # [-] + bicycle: true # [-] + motorcycle: true # [-] + pedestrian: true # [-] # safety check configuration enable: true # [-] check_current_lane: false # [-] @@ -158,13 +174,13 @@ # collision check parameters check_all_predicted_path: false # [-] safety_check_backward_distance: 100.0 # [m] - hysteresis_factor_expand_rate: 2.0 # [-] + hysteresis_factor_expand_rate: 1.5 # [-] hysteresis_factor_safe_count: 10 # [-] # predicted path parameters min_velocity: 1.38 # [m/s] max_velocity: 50.0 # [m/s] time_resolution: 0.5 # [s] - time_horizon_for_front_object: 10.0 # [s] + time_horizon_for_front_object: 3.0 # [s] time_horizon_for_rear_object: 10.0 # [s] delay_until_departure: 0.0 # [s] # rss parameters @@ -172,7 +188,7 @@ expected_rear_deceleration: -1.0 # [m/ss] rear_vehicle_reaction_time: 2.0 # [s] rear_vehicle_safety_time_margin: 1.0 # [s] - lateral_distance_max_threshold: 2.0 # [m] + lateral_distance_max_threshold: 0.75 # [m] longitudinal_distance_min_threshold: 3.0 # [m] longitudinal_velocity_delta_time: 0.8 # [s] @@ -180,10 +196,10 @@ avoidance: # avoidance lateral parameters lateral: - lateral_execution_threshold: 0.499 # [m] + lateral_execution_threshold: 0.09 # [m] lateral_small_shift_threshold: 0.101 # [m] lateral_avoid_check_threshold: 0.1 # [m] - soft_road_shoulder_margin: 0.8 # [m] + soft_road_shoulder_margin: 0.3 # [m] hard_road_shoulder_margin: 0.3 # [m] max_right_shift_length: 5.0 max_left_shift_length: 5.0 @@ -215,6 +231,10 @@ stop_buffer: 1.0 # [m] policy: + # policy for rtc request. select "per_shift_line" or "per_avoidance_maneuver". + # "per_shift_line": request approval for each shift line. + # "per_avoidance_maneuver": request approval for avoidance maneuver (avoid + return). + make_approval_request: "per_shift_line" # policy for vehicle slow down behavior. select "best_effort" or "reliable". # "best_effort": slow down deceleration & jerk are limited by constraints. # but there is a possibility that the vehicle can't stop in front of the vehicle. @@ -242,13 +262,13 @@ longitudinal: nominal_deceleration: -1.0 # [m/ss] nominal_jerk: 0.5 # [m/sss] - max_deceleration: -2.0 # [m/ss] + max_deceleration: -1.5 # [m/ss] max_jerk: 1.0 # [m/sss] max_acceleration: 1.0 # [m/ss] shift_line_pipeline: trim: - quantize_filter_threshold: 0.2 + quantize_filter_threshold: 0.1 same_grad_filter_1_threshold: 0.1 same_grad_filter_2_threshold: 0.2 same_grad_filter_3_threshold: 0.5 diff --git a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml index 5ea50f90ab62c..9518185d30d63 100644 --- a/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml +++ b/planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml @@ -7,7 +7,6 @@ # avoidance is performed for the object type with true target_object: car: - is_target: true # [-] execute_num: 2 # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] @@ -16,7 +15,6 @@ avoid_margin_lateral: 1.0 # [m] safety_buffer_lateral: 0.7 # [m] truck: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -25,7 +23,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 bus: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -34,7 +31,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 trailer: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -43,7 +39,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.7 unknown: - is_target: true execute_num: 1 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -52,7 +47,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 0.0 bicycle: - is_target: true execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -61,7 +55,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 motorcycle: - is_target: true execute_num: 2 moving_speed_threshold: 1.0 # 3.6km/h moving_time_threshold: 1.0 @@ -70,7 +63,6 @@ avoid_margin_lateral: 1.0 safety_buffer_lateral: 1.0 pedestrian: - is_target: true execute_num: 2 moving_speed_threshold: 0.28 # 1.0km/h moving_time_threshold: 1.0 @@ -80,3 +72,16 @@ safety_buffer_lateral: 1.0 lower_distance_for_polygon_expansion: 0.0 # [m] upper_distance_for_polygon_expansion: 1.0 # [m] + + # For target object filtering + target_filtering: + # avoidance target type + target_type: + car: true # [-] + truck: true # [-] + bus: true # [-] + trailer: true # [-] + unknown: true # [-] + bicycle: false # [-] + motorcycle: false # [-] + pedestrian: false # [-] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml deleted file mode 100644 index 896c44c9cec9a..0000000000000 --- a/planning/behavior_path_planner/config/dynamic_avoidance.param.yaml +++ /dev/null @@ -1,58 +0,0 @@ -/**: - ros__parameters: - dynamic_avoidance: - common: - enable_debug_info: true - - # avoidance is performed for the object type with true - target_object: - car: true - truck: true - bus: true - trailer: true - unknown: false - bicycle: false - motorcycle: true - pedestrian: false - - min_obstacle_vel: 0.0 # [m/s] - - successive_num_to_entry_dynamic_avoidance_condition: 5 - successive_num_to_exit_dynamic_avoidance_condition: 1 - - min_obj_lat_offset_to_ego_path: 0.0 # [m] - max_obj_lat_offset_to_ego_path: 1.0 # [m] - - cut_in_object: - min_time_to_start_cut_in: 1.0 # [s] - min_lon_offset_ego_to_object: 0.0 # [m] - - cut_out_object: - max_time_from_outside_ego_path: 2.0 # [s] - min_object_lat_vel: 0.3 # [m/s] - - crossing_object: - min_overtaking_object_vel: 1.0 - max_overtaking_object_angle: 1.05 - min_oncoming_object_vel: 0.0 - max_oncoming_object_angle: 0.523 - - front_object: - max_object_angle: 0.785 - - drivable_area_generation: - lat_offset_from_obstacle: 0.8 # [m] - max_lat_offset_to_avoid: 0.5 # [m] - - # for same directional object - overtaking_object: - max_time_to_collision: 40.0 # [s] - start_duration_to_avoid: 2.0 # [s] - end_duration_to_avoid: 4.0 # [s] - duration_to_hold_avoidance: 3.0 # [s] - - # for opposite directional object - oncoming_object: - max_time_to_collision: 15.0 # [s] - start_duration_to_avoid: 12.0 # [s] - end_duration_to_avoid: 0.0 # [s] diff --git a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml index 127b70fbf7bcb..854c29aa89bc5 100644 --- a/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml +++ b/planning/behavior_path_planner/config/dynamic_avoidance/dynamic_avoidance.param.yaml @@ -28,10 +28,12 @@ cut_in_object: min_time_to_start_cut_in: 1.0 # [s] min_lon_offset_ego_to_object: 0.0 # [m] + min_object_vel: 0.5 # [m/s] cut_out_object: max_time_from_outside_ego_path: 2.0 # [s] min_object_lat_vel: 0.3 # [m/s] + min_object_vel: 0.5 # [m/s] crossing_object: min_overtaking_object_vel: 1.0 @@ -41,9 +43,11 @@ front_object: max_object_angle: 0.785 + min_object_vel: -0.5 # [m/s] The value is negative considering the noisy velocity of the stopped vehicle. + max_ego_path_lat_cover_ratio: 0.3 # [-] The object will be ignored if the ratio of the object covering the ego's path is higher than this value. drivable_area_generation: - polygon_generation_method: "object_path_base" # choose "ego_path_base" and "object_path_base" + polygon_generation_method: "ego_path_base" # choose "ego_path_base" and "object_path_base" object_path_base: min_longitudinal_polygon_margin: 3.0 # [m] diff --git a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml index 5ffac49847ce6..0ab617a1a7ab9 100644 --- a/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml +++ b/planning/behavior_path_planner/config/goal_planner/goal_planner.param.yaml @@ -119,8 +119,7 @@ # EgoPredictedPath ego_predicted_path: min_velocity: 0.0 - acceleration: 1.0 - max_velocity: 1.0 + min_acceleration: 1.0 time_horizon_for_front_object: 10.0 time_horizon_for_rear_object: 10.0 time_resolution: 0.5 @@ -160,6 +159,8 @@ safety_check_params: # safety check configuration enable_safety_check: true + method: "integral_predicted_polygon" + keep_unsafe_time: 3.0 # collision check parameters check_all_predicted_path: true publish_debug_marker: false @@ -169,10 +170,15 @@ lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 longitudinal_velocity_delta_time: 0.8 + integral_predicted_polygon_params: + forward_margin: 1.0 + backward_margin: 1.0 + lat_margin: 1.0 + time_horizon: 10.0 # hysteresis factor to expand/shrink polygon with the value hysteresis_factor_expand_rate: 1.0 # temporary - backward_path_length: 30.0 + backward_path_length: 100.0 forward_path_length: 100.0 # debug diff --git a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml index 3a94a18e011b9..4bcb847d1280c 100644 --- a/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml +++ b/planning/behavior_path_planner/config/start_planner/start_planner.param.yaml @@ -2,15 +2,13 @@ ros__parameters: start_planner: - verbose: false - th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 th_moving_object_velocity: 1.0 - th_distance_to_middle_of_the_road: 0.1 + th_distance_to_middle_of_the_road: 0.5 center_line_path_interval: 1.0 # shift pull out enable_shift_pull_out: true @@ -24,7 +22,7 @@ maximum_curvature: 0.07 # geometric pull out enable_geometric_pull_out: true - divide_pull_out_path: false + divide_pull_out_path: true geometric_pull_out_velocity: 1.0 arc_path_interval: 1.0 lane_departure_margin: 0.2 @@ -85,8 +83,7 @@ # EgoPredictedPath ego_predicted_path: min_velocity: 0.0 - acceleration: 1.0 - max_velocity: 1.0 + min_acceleration: 1.0 time_horizon_for_front_object: 10.0 time_horizon_for_rear_object: 10.0 time_resolution: 0.5 @@ -140,3 +137,7 @@ # temporary backward_path_length: 30.0 forward_path_length: 100.0 + + # debug + debug: + print_debug_info: false diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index da96567ed542a..c4580bb1e82c3 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -209,6 +209,149 @@ The closer the object is to the shoulder, the larger the value of $ratio$ (theor In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted). +### Flowchart + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title object filtering flowchart +start + +if(object is satisfied with common target condition ?) then (yes) +if(vehicle can pass by the object without avoidance ?) then (yes) +:return false; +stop +else (\n no) +endif +else (\n no) +:return false; +stop +endif + +if(object is vehicle type ?) then (yes) +if(object is satisfied with vehicle type target condition ?) then (yes) +else (\n no) +:return false; +stop +endif +else (\n no) +if(object is satisfied with non-vehicle type target condition ?) then (yes) +else (\n no) +:return false; +stop +endif +endif + +#FF006C :return true; +stop +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for all types object +start + +partition isSatisfiedWithCommonCondition() { +if(object is avoidance target type ?) then (yes) +if(object is moving more than threshold time ?) then (yes) +:return false; +stop +else (\n no) +if(object is farther than forward distance threshold ?) then (yes) +:return false; +stop +else (\n no) +If(object is farther than backward distance threshold ?) then (yes) +:return false; +stop +else (\n no) +endif +endif +endif +else (\n no) +:return false; +stop +endif +#FF006C :return true; +stop +} + +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for vehicle type objects +start + +partition isSatisfiedWithVehicleCodition() { +if(object is force avoidance target ?) then (yes) +#FF006C :return true; +stop +else (\n no) +if(object is nearer lane centerline than threshold ?) then (yes) +:return false; +stop +else (\n no) +if(object is on same lane for ego ?) then (yes) +if(object is shifting right or left side road shoulder more than threshold ?) then (yes) +#FF006C :return true; +stop +else (\n no) +:return false; +stop +endif +else (\n no) +if(object is in intersection ?) then (no) +#FF006C :return true; +stop +else (\n yes) +if(object pose is paralell to ego lane ?) then (no) +#FF006C :return true; +stop +else (\n yes) +:return false; +stop +endif +endif +endif +endif +endif +} + +@enduml +``` + +```plantuml +@startuml +skinparam defaultTextAlignment center +skinparam noteTextAlignment left + +title filtering flow for non-vehicle type objects +start + +partition isSatisfiedWithNonVehicleCodition() { +if(object is nearer crosswalk than threshold ?) then (yes) +:return false; +stop +else (\n no) +endif +#FF006C :return true; +stop +} + +@enduml +``` + ## Overview of algorithm for avoidance path generation ### How to prevent shift line chattering that is caused by perception noise diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md index d924be2f63aba..5af70a4f98e29 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md @@ -575,7 +575,7 @@ In addition, while manual driving, the manager always updates **root lanelet** b std::max(p.backward_path_length, p.backward_path_length + extra_margin); const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + root_lanelet_.value(), pose, backward_length, std::numeric_limits::max()); lanelet::ConstLanelet closest_lane{}; if (lanelet::utils::query::getClosestLaneletWithConstrains( diff --git a/planning/behavior_path_planner/image/behavior_modules.png b/planning/behavior_path_planner/image/behavior_modules.png deleted file mode 100644 index 0f21b959ed32c..0000000000000 Binary files a/planning/behavior_path_planner/image/behavior_modules.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png b/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png deleted file mode 100644 index 72b6fbf36d119..0000000000000 Binary files a/planning/behavior_path_planner/image/behavior_path_planner_bt_config.png and /dev/null differ diff --git a/planning/behavior_path_planner/image/checking_module_transition.png b/planning/behavior_path_planner/image/checking_module_transition.png new file mode 100644 index 0000000000000..5d933aa0b0718 Binary files /dev/null and b/planning/behavior_path_planner/image/checking_module_transition.png differ diff --git a/planning/behavior_path_planner/image/static_drivable_area_after_expansion.png b/planning/behavior_path_planner/image/static_drivable_area_after_expansion.png new file mode 100644 index 0000000000000..c01ca440072b5 Binary files /dev/null and b/planning/behavior_path_planner/image/static_drivable_area_after_expansion.png differ diff --git a/planning/behavior_path_planner/image/static_drivable_area_before_expansion.png b/planning/behavior_path_planner/image/static_drivable_area_before_expansion.png new file mode 100644 index 0000000000000..4f78f12bb16a2 Binary files /dev/null and b/planning/behavior_path_planner/image/static_drivable_area_before_expansion.png differ diff --git a/planning/behavior_path_planner/image/supported_module_avoidance.svg b/planning/behavior_path_planner/image/supported_module_avoidance.svg new file mode 100644 index 0000000000000..7d566ca3ba20a --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_avoidance.svg @@ -0,0 +1,58 @@ + + + + + + + + + + + + + + + + +
+
+
+ Avoidance +
+
+
+ + Avoidance + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg b/planning/behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg new file mode 100644 index 0000000000000..42d860d7546db --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_avoidance_by_lane_change.svg @@ -0,0 +1,58 @@ + + + + + + + + + + +
+
+
+ Avoidance by Lane Change +
+
+
+ + Avoidance by Lane Change + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/supported_module_goal_planner.svg b/planning/behavior_path_planner/image/supported_module_goal_planner.svg new file mode 100644 index 0000000000000..6b790676a0019 --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_goal_planner.svg @@ -0,0 +1,57 @@ + + + + + + + + + + + + + + + + +
+
+
+ Goal Planner +
+
+
+ + Goal Planner + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/supported_module_lane_change.svg b/planning/behavior_path_planner/image/supported_module_lane_change.svg new file mode 100644 index 0000000000000..eb896a0f7ee98 --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_lane_change.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + +
+
+
+ Lane Change +
+
+
+ + Lane Change + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/supported_module_lane_following.svg b/planning/behavior_path_planner/image/supported_module_lane_following.svg new file mode 100644 index 0000000000000..8b850b4595122 --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_lane_following.svg @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + +
+
+
+ Lane Following +
+
+
+ + Lane Following + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/supported_module_start_planner.svg b/planning/behavior_path_planner/image/supported_module_start_planner.svg new file mode 100644 index 0000000000000..48d588dff4463 --- /dev/null +++ b/planning/behavior_path_planner/image/supported_module_start_planner.svg @@ -0,0 +1,48 @@ + + + + + + + + + + +
+
+
+ Start Planner +
+
+
+ + Start Planner + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 1a9b23be00a3f..c47dc70213d1a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -15,13 +15,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ #define BEHAVIOR_PATH_PLANNER__BEHAVIOR_PATH_PLANNER_NODE_HPP_ -#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/steering_factor_interface.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" #include #include #include @@ -35,7 +34,6 @@ #include #include #include -#include #include #include #include @@ -66,7 +64,6 @@ using nav_msgs::msg::Odometry; using rcl_interfaces::msg::SetParametersResult; using steering_factor_interface::SteeringFactorInterface; using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using tier4_planning_msgs::msg::LateralOffset; using tier4_planning_msgs::msg::RerouteAvailability; using tier4_planning_msgs::msg::Scenario; @@ -171,13 +168,12 @@ class BehaviorPathPlannerNode : public rclcpp::Node // debug rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; - rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_turn_signal_info_publisher_; /** * @brief publish reroute availability */ - void publish_reroute_availability(); + void publish_reroute_availability() const; /** * @brief publish steering factor from intersection diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp index 100e8046f8d7f..12b54a5031041 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/avoidance/debug.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__AVOIDANCE__DEBUG_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include @@ -39,6 +39,7 @@ using behavior_path_planner::AvoidanceState; using behavior_path_planner::AvoidLineArray; using behavior_path_planner::DebugData; using behavior_path_planner::ObjectDataArray; +using behavior_path_planner::PathShifter; using behavior_path_planner::ShiftLineArray; using geometry_msgs::msg::Point; using geometry_msgs::msg::Polygon; @@ -65,6 +66,8 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( const lanelet::ConstLineStrings3d & linestrings, std::string && ns, const float & r, const float & g, const float & b); +MarkerArray createDebugMarkerArray( + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug); } // namespace marker_utils::avoidance_marker std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sp_arr); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp index 1d0c9f5c2ce65..96f0851ed33c0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/lane_change/debug.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ #define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__LANE_CHANGE__DEBUG_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp index 1d1ad56342f4d..7ceafb6f428de 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp @@ -15,12 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ #define BEHAVIOR_PATH_PLANNER__PLANNER_MANAGER_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" +#include #include #include @@ -104,14 +105,17 @@ class PlannerManager /** * @brief register managers. - * @param manager pointer. + * @param node. + * @param plugin name. */ - void registerSceneModuleManager(const SceneModuleManagerPtr & manager_ptr) - { - RCLCPP_INFO(logger_, "register %s module", manager_ptr->name().c_str()); - manager_ptrs_.push_back(manager_ptr); - processing_time_.emplace(manager_ptr->name(), 0.0); - } + void launchScenePlugin(rclcpp::Node & node, const std::string & name); + + /** + * @brief unregister managers. + * @param node. + * @param plugin name. + */ + void removeScenePlugin(rclcpp::Node & node, const std::string & name); /** * @brief update module parameters. used for dynamic reconfigure. @@ -132,7 +136,7 @@ class PlannerManager std::for_each(manager_ptrs_.begin(), manager_ptrs_.end(), [](const auto & m) { m->reset(); }); approved_module_ptrs_.clear(); candidate_module_ptrs_.clear(); - root_lanelet_ = boost::none; + root_lanelet_ = std::nullopt; resetProcessingTime(); } @@ -222,6 +226,13 @@ class PlannerManager */ bool hasApprovedModules() const { return !approved_module_ptrs_.empty(); } + bool hasNonAlwaysExecutableApprovedModules() const + { + return std::any_of( + approved_module_ptrs_.begin(), approved_module_ptrs_.end(), + [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); + } + /** * @brief check if there are candidate modules. */ @@ -275,6 +286,8 @@ class PlannerManager module_ptr->publishRTCStatus(); + module_ptr->publishObjectsOfInterestMarker(); + processing_time_.at(module_ptr->name()) += stop_watch_.toc(module_ptr->name(), true); return result; @@ -298,6 +311,7 @@ class PlannerManager { module_ptr->onExit(); module_ptr->publishRTCStatus(); + module_ptr->publishObjectsOfInterestMarker(); module_ptr.reset(); } @@ -421,7 +435,7 @@ class PlannerManager std::string getNames(const std::vector & modules) const; - boost::optional root_lanelet_{boost::none}; + std::optional root_lanelet_{std::nullopt}; std::vector manager_ptrs_; @@ -431,6 +445,8 @@ class PlannerManager std::unique_ptr debug_publisher_ptr_; + pluginlib::ClassLoader plugin_loader_; + mutable rclcpp::Logger logger_; mutable rclcpp::Clock clock_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index cbf40e59535be..b01cb62f22372 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -15,11 +15,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_MODULE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/helper.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include @@ -44,12 +45,16 @@ using motion_utils::findNearestIndex; using tier4_planning_msgs::msg::AvoidanceDebugMsg; +using helper::avoidance::AvoidanceHelper; + class AvoidanceModule : public SceneModuleInterface { public: AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); CandidateOutput planCandidate() const override; BehaviorModuleOutput plan() override; @@ -118,7 +123,7 @@ class AvoidanceModule : public SceneModuleInterface if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); + PlanningBehavior::AVOIDANCE, SteeringFactor::LEFT, SteeringFactor::TURNING, ""); } } @@ -132,8 +137,7 @@ class AvoidanceModule : public SceneModuleInterface if (finish_distance > -1.0e-03) { steering_factor_interface_ptr_->updateSteeringFactor( {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, SteeringFactor::RIGHT, SteeringFactor::TURNING, - ""); + PlanningBehavior::AVOIDANCE, SteeringFactor::RIGHT, SteeringFactor::TURNING, ""); } } } @@ -219,6 +223,11 @@ class AvoidanceModule : public SceneModuleInterface */ void insertStopPoint(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; + /** + * @brief insert stop point in return path to original lane. + * @param flag. if it is true, the ego decelerates within accel/jerk constraints. + * @param target path. + */ void insertReturnDeadLine(const bool use_constraints_for_decel, ShiftedPath & shifted_path) const; /** @@ -286,167 +295,12 @@ class AvoidanceModule : public SceneModuleInterface */ void fillDebugData(const AvoidancePlanningData & data, DebugData & debug) const; - /** - * @brief update registered shift lines. - * @param current shift lines. - */ - void registerRawShiftLines(const AvoidLineArray & future_registered); - - /** - * @brief update path index of the registered objects. remove old objects whose end point is - * behind ego pose. - */ - void updateRegisteredRawShiftLines(); - /** * @brief check whether the ego can transit yield maneuver. * @param avoidance data. */ bool canYieldManeuver(const AvoidancePlanningData & data) const; - // shift line generation - - /** - * @brief Calculate the shift points (start/end point, shift length) from the object lateral - * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here. - * @param avoidance data. - * @param debug data. - * @return processed shift lines. - */ - AvoidOutlines generateAvoidOutline(AvoidancePlanningData & data, DebugData & debug) const; - - /* - * @brief merge avoid outlines. - * @param original shift lines. - * @param debug data. - * @return processed shift lines. - */ - AvoidOutlines applyMergeProcess(const AvoidOutlines & outlines, DebugData & debug) const; - - /* - * @brief fill gap between two shift lines. - * @param original shift lines. - * @param debug data. - * @return processed shift lines. - */ - AvoidOutlines applyFillGapProcess(const AvoidOutlines & outlines, DebugData & debug) const; - - /* - * @brief generate candidate shift lines. - * @param one-shot shift lines. - * @param debug data. - */ - AvoidLineArray generateCandidateShiftLine( - const AvoidLineArray & shift_lines, DebugData & debug) const; - - /** - * @brief clean up raw shift lines. - * @param target shift lines. - * @param debug data. - * @return processed shift lines. - * @details process flow: - * 1. combine raw shirt lines and previous registered shift lines. - * 2. add return shift line. - * 3. merge raw shirt lines. - * 4. trim unnecessary shift lines. - */ - AvoidLineArray applyPreProcess(const AvoidOutlines & outlines, DebugData & debug) const; - - /* - * @brief fill gap among shift lines. - * @param original shift lines. - * @param debug data. - * @return processed shift lines. - */ - AvoidLineArray applyFillGapProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; - - /* - * @brief merge negative & positive shift lines. - * @param original shift lines. - * @param debug data. - * @return processed shift lines. - */ - AvoidLineArray applyMergeProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; - - /* - * @brief add return shift line from ego position. - * @param current raw shift line. - * @param current registered shift line. - * @param debug data. - */ - AvoidLineArray applyCombineProcess( - const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, - [[maybe_unused]] DebugData & debug) const; - - /* - * @brief add return shift line from ego position. - * @param shift lines which the return shift is added. - * Pick up the last shift point, which is the most farthest from ego, from the current candidate - * avoidance points and registered points in the shifter. If the last shift length of the point is - * non-zero, add a return-shift to center line from the point. If there is no shift point in - * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to - * center line from ego. - */ - AvoidLineArray addReturnShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; - - /* - * @brief extract shift lines from total shift lines based on their gradient. - * @param shift length data. - * @return extracted shift lines. - */ - AvoidLineArray extractShiftLinesFromLine(ShiftLineData & shift_line_data) const; - - /* - * @brief remove unnecessary avoid points - * @param original shift lines. - * @param debug data. - * @return processed shift lines. - * @details - * - Combine avoid points that have almost same gradient - * - Quantize the shift length to reduce the shift point noise - * - Change the shift length to the previous one if the deviation is small. - * - Remove unnecessary return shift (back to the center line). - */ - AvoidLineArray applyTrimProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; - - /* - * @brief extract new shift lines based on current shifted path. the module makes a RTC request - * for those new shift lines. - * @param candidate shift lines. - * @return new shift lines. - */ - AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; - - /* - * @brief generate total shift line. total shift line has shift length and gradient array. - * @param raw shift lines. - * @param total shift line. - */ - void generateTotalShiftLine( - const AvoidLineArray & avoid_points, ShiftLineData & shift_line_data) const; - - /* - * @brief quantize shift line length. - * @param target shift lines. - * @param threshold. shift length is quantized by this value. (if it is 0.3[m], the output shift - * length is 0.0, 0.3, 0.6...) - */ - void applyQuantizeProcess(AvoidLineArray & shift_lines, const double threshold) const; - - /* - * @brief trim shift line whose relative longitudinal distance is less than threshold. - * @param target shift lines. - * @param threshold. - */ - void applySmallShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; - - /* - * @brief merge multiple shift lines whose relative gradient is less than threshold. - * @param target shift lines. - * @param threshold. - */ - void applySimilarGradFilter(AvoidLineArray & shift_lines, const double threshold) const; - /** * @brief add new shift line to path shifter if the RTC status is activated. * @param new shift lines. @@ -503,15 +357,6 @@ class AvoidanceModule : public SceneModuleInterface */ bool isSafePath(ShiftedPath & shifted_path, DebugData & debug) const; - bool isComfortable(const AvoidLineArray & shift_lines) const - { - return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { - return PathShifter::calcJerkFromLatLonDistance( - line.getRelativeLength(), line.getRelativeLongitudinal(), - helper_.getAvoidanceEgoSpeed()) < helper_.getLateralMaxJerkLimit(); - }); - } - // post process /** @@ -541,8 +386,7 @@ class AvoidanceModule : public SceneModuleInterface removeRTCStatus(); } - current_raw_shift_lines_.clear(); - registered_raw_shift_lines_.clear(); + generator_.reset(); path_shifter_.setShiftLines(ShiftLineArray{}); } @@ -556,15 +400,6 @@ class AvoidanceModule : public SceneModuleInterface path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); } - // misc functions - - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - - // TODO(Horibe): think later. - // for unique ID - mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable - uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; } - struct RegisteredShiftLine { UUID uuid; @@ -580,9 +415,11 @@ class AvoidanceModule : public SceneModuleInterface bool safe_{true}; + std::shared_ptr helper_; + std::shared_ptr parameters_; - helper::avoidance::AvoidanceHelper helper_; + utils::avoidance::ShiftLineGenerator generator_; AvoidancePlanningData avoid_data_; @@ -592,10 +429,6 @@ class AvoidanceModule : public SceneModuleInterface RegisteredShiftLineArray right_shift_array_; - AvoidLineArray registered_raw_shift_lines_; - - AvoidLineArray current_raw_shift_lines_; - UUID candidate_uuid_; ObjectDataArray registered_objects_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp index 9d89138f7cdfe..0fe8bd7efa17b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp @@ -16,8 +16,8 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__MANAGER_HPP_ #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -32,12 +32,15 @@ namespace behavior_path_planner class AvoidanceModuleManager : public SceneModuleManagerInterface { public: - AvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + AvoidanceModuleManager() : SceneModuleManagerInterface{"avoidance"} {} + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp index 077e0ecec0f29..e37ef27d44426 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__DYNAMIC_AVOIDANCE_MODULE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" #include #include @@ -46,6 +46,11 @@ struct MinMaxValue double max_value{0.0}; }; +enum class PolygonGenerationMethod { + EGO_PATH_BASE = 0, + OBJECT_PATH_BASE, +}; + struct DynamicAvoidanceParameters { // common @@ -71,15 +76,20 @@ struct DynamicAvoidanceParameters double min_time_to_start_cut_in{0.0}; double min_lon_offset_ego_to_cut_in_object{0.0}; + double min_cut_in_object_vel{0.0}; double max_time_from_outside_ego_path_for_cut_out{0.0}; double min_cut_out_object_lat_vel{0.0}; + double min_cut_out_object_vel{0.0}; double max_front_object_angle{0.0}; + double min_front_object_vel{0.0}; + double max_front_object_ego_path_lat_cover_ratio{0.0}; double min_overtaking_crossing_object_vel{0.0}; double max_overtaking_crossing_object_angle{0.0}; double min_oncoming_crossing_object_vel{0.0}; double max_oncoming_crossing_object_angle{0.0}; // drivable area generation + PolygonGenerationMethod polygon_generation_method{}; double min_obj_path_based_lon_polygon_margin{0.0}; double lat_offset_from_obstacle{0.0}; double max_lat_offset_to_avoid{0.0}; @@ -99,10 +109,6 @@ struct DynamicAvoidanceParameters class DynamicAvoidanceModule : public SceneModuleInterface { public: - enum class PolygonGenerationMethod { - EGO_PATH_BASE = 0, - OBJECT_PATH_BASE, - }; struct DynamicAvoidanceObject { DynamicAvoidanceObject( @@ -137,18 +143,15 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::optional lat_offset_to_avoid{std::nullopt}; bool is_collision_left{false}; bool should_be_avoided{false}; - PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::OBJECT_PATH_BASE}; void update( const MinMaxValue & arg_lon_offset_to_avoid, const MinMaxValue & arg_lat_offset_to_avoid, - const bool arg_is_collision_left, const bool arg_should_be_avoided, - const PolygonGenerationMethod & arg_polygon_generation_method) + const bool arg_is_collision_left, const bool arg_should_be_avoided) { lon_offset_to_avoid = arg_lon_offset_to_avoid; lat_offset_to_avoid = arg_lat_offset_to_avoid; is_collision_left = arg_is_collision_left; should_be_avoided = arg_should_be_avoided; - polygon_generation_method = arg_polygon_generation_method; } }; @@ -242,12 +245,11 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateObject( const std::string & uuid, const MinMaxValue & lon_offset_to_avoid, const MinMaxValue & lat_offset_to_avoid, const bool is_collision_left, - const bool should_be_avoided, const PolygonGenerationMethod & polygon_generation_method) + const bool should_be_avoided) { if (object_map_.count(uuid) != 0) { object_map_.at(uuid).update( - lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, - polygon_generation_method); + lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided); } } @@ -266,7 +268,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { @@ -305,8 +309,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface void updateTargetObjects(); bool willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, - PolygonGenerationMethod & polygon_generation_method) const; + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const; DecisionWithReason willObjectCutOut( const double obj_tangent_vel, const double obj_normal_vel, const bool is_object_left, const std::optional & prev_object) const; @@ -320,13 +323,17 @@ class DynamicAvoidanceModule : public SceneModuleInterface LatLonOffset getLateralLongitudinalOffset( const std::vector & ego_path, const geometry_msgs::msg::Pose & obj_pose, const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; + double calcValidStartLengthToAvoid( + const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const; MinMaxValue calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & path_points_for_object_polygon, + const std::vector & input_ref_path_points, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double time_to_collision) const; - MinMaxValue calcMinMaxLateralOffsetToAvoid( - const std::vector & path_points_for_object_polygon, - const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, + std::optional calcMinMaxLateralOffsetToAvoid( + const std::vector & input_ref_path_points, const Polygon2d & obj_points, + const double obj_vel, const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const; std::pair getAdjacentLanes( @@ -346,6 +353,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface std::vector target_objects_; // std::vector prev_target_objects_; + std::vector prev_input_ref_path_points; std::shared_ptr parameters_; TargetObjectsManager target_objects_manager_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp index 10a0d055a5a77..c02ee88d3fa3e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__DYNAMIC_AVOIDANCE__MANAGER_HPP_ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -31,13 +31,15 @@ namespace behavior_path_planner class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface { public: - DynamicAvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + DynamicAvoidanceModuleManager() : SceneModuleManagerInterface{"dynamic_avoidance"} {} + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { return std::make_unique( - name_, *node_, parameters_, rtc_interface_ptr_map_); + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index b04a3fee41dcb..a9bc31983ac6c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__GOAL_PLANNER_MODULE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" #include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" #include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" @@ -24,9 +23,10 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" #include "behavior_path_planner/utils/occupancy_grid_based_collision_detector/occupancy_grid_based_collision_detector.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -62,7 +62,6 @@ using freespace_planning_algorithms::RRTStar; using freespace_planning_algorithms::RRTStarParam; using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; -using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; @@ -233,18 +232,26 @@ struct LastApprovalData struct PreviousPullOverData { + struct SafetyStatus + { + std::optional safe_start_time{}; + bool is_safe{false}; + }; + void reset() { stop_path = nullptr; stop_path_after_approval = nullptr; found_path = false; - is_safe = false; + safety_status = SafetyStatus{}; + has_decided_path = false; } std::shared_ptr stop_path{nullptr}; std::shared_ptr stop_path_after_approval{nullptr}; bool found_path{false}; - bool is_safe{false}; + SafetyStatus safety_status{}; + bool has_decided_path{false}; }; class GoalPlannerModule : public SceneModuleInterface @@ -253,7 +260,9 @@ class GoalPlannerModule : public SceneModuleInterface GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { @@ -471,11 +480,13 @@ class GoalPlannerModule : public SceneModuleInterface void updateSafetyCheckTargetObjectsData( const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, const std::vector & ego_predicted_path) const; - bool isSafePath() const; - bool checkSafetyWithRSS( - const PathWithLaneId & planned_path, - const std::vector & ego_predicted_path, - const std::vector & objects, const double hysteresis_factor) const; + /** + * @brief Checks if the current path is safe. + * @return std::pair + * first: If the path is safe for a certain period of time, true. + * second: If the path is safe in the current state, true. + */ + std::pair isSafePath() const; // debug void setDebugData(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp index 4a7eecfc68caf..a166a677c7388 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__GOAL_PLANNER__MANAGER_HPP_ #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -30,12 +30,15 @@ namespace behavior_path_planner class GoalPlannerModuleManager : public SceneModuleManagerInterface { public: - GoalPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + GoalPlannerModuleManager() : SceneModuleManagerInterface{"goal_planner"} {} + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp index 4b548c86e55bb..67fd2c1ba0c77 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp @@ -18,15 +18,12 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" #include -#include namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; using AvoidanceDebugData = DebugData; class AvoidanceByLaneChange : public NormalLaneChange diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index daad5b1a40610..be49ec5b2e740 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -14,31 +14,24 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_ -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include #include -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" #include #include #include -#include -#include #include #include #include -#include namespace behavior_path_planner { @@ -50,8 +43,6 @@ using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; using tier4_autoware_utils::StopWatch; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class LaneChangeBase { @@ -107,6 +98,8 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; + virtual bool isStoppedAtRedTrafficLight() const = 0; + virtual bool calcAbortPath() = 0; virtual bool specialRequiredCheck() const { return false; } @@ -214,9 +207,9 @@ class LaneChangeBase return direction_; } - boost::optional getStopPose() const { return lane_change_stop_pose_; } + std::optional getStopPose() const { return lane_change_stop_pose_; } - void resetStopPose() { lane_change_stop_pose_ = boost::none; } + void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; @@ -254,7 +247,7 @@ class LaneChangeBase PathWithLaneId prev_module_path_{}; DrivableAreaInfo prev_drivable_area_info_{}; TurnSignalInfo prev_turn_signal_info_{}; - boost::optional lane_change_stop_pose_{boost::none}; + std::optional lane_change_stop_pose_{std::nullopt}; PathWithLaneId prev_approved_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp index b7f84987e0499..2e3cbd992e27f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp @@ -15,22 +15,18 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__INTERFACE_HPP_ -#include "behavior_path_planner/marker_utils/lane_change/debug.hpp" #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include "behavior_path_planner/scene_module/lane_change/external_request.hpp" #include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" #include #include #include @@ -40,23 +36,23 @@ #include #include #include -#include -#include namespace behavior_path_planner { using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; class LaneChangeInterface : public SceneModuleInterface { public: LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map > & rtc_interface_ptr_map, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, std::unique_ptr && module_type); LaneChangeInterface(const LaneChangeInterface &) = delete; @@ -84,9 +80,10 @@ class LaneChangeInterface : public SceneModuleInterface CandidateOutput planCandidate() const override; - std::shared_ptr get_debug_msg_array() const; - - void acceptVisitor(const std::shared_ptr & visitor) const override; + void acceptVisitor( + [[maybe_unused]] const std::shared_ptr & visitor) const override + { + } void updateModuleParams(const std::any & parameters) override; @@ -139,7 +136,6 @@ class LaneChangeInterface : public SceneModuleInterface const CandidateOutput & output, const LaneChangePath & selected_path) const; mutable MarkerArray virtual_wall_marker_; - mutable LaneChangeDebugMsgArray lane_change_debug_msg_array_; std::unique_ptr prev_approved_path_; @@ -157,7 +153,9 @@ class AvoidanceByLaneChangeInterface : public LaneChangeInterface const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map > & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); bool isExecutionRequested() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index cf618b755b094..ed476adcbd834 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -16,7 +16,7 @@ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__MANAGER_HPP_ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include "route_handler/route_handler.hpp" #include @@ -34,8 +34,12 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface { public: LaneChangeModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const Direction direction, const LaneChangeModuleType type); + const std::string & name, const Direction direction, const LaneChangeModuleType type) + : SceneModuleManagerInterface{name}, direction_{direction}, type_{type} + { + } + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override; @@ -49,15 +53,61 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface LaneChangeModuleType type_; }; +class LaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + LaneChangeRightModuleManager() + : LaneChangeModuleManager( + "lane_change_right", route_handler::Direction::RIGHT, LaneChangeModuleType::NORMAL) + { + } +}; + +class LaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + LaneChangeLeftModuleManager() + : LaneChangeModuleManager( + "lane_change_left", route_handler::Direction::LEFT, LaneChangeModuleType::NORMAL) + { + } +}; + +class ExternalRequestLaneChangeRightModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeRightModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_right", route_handler::Direction::RIGHT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } +}; + +class ExternalRequestLaneChangeLeftModuleManager : public LaneChangeModuleManager +{ +public: + ExternalRequestLaneChangeLeftModuleManager() + : LaneChangeModuleManager( + "external_request_lane_change_left", route_handler::Direction::LEFT, + LaneChangeModuleType::EXTERNAL_REQUEST) + { + } +}; + class AvoidanceByLaneChangeModuleManager : public LaneChangeModuleManager { public: - AvoidanceByLaneChangeModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + AvoidanceByLaneChangeModuleManager() + : LaneChangeModuleManager( + "avoidance_by_lc", route_handler::Direction::NONE, + LaneChangeModuleType::AVOIDANCE_BY_LANE_CHANGE) + { + } - std::unique_ptr createNewSceneModuleInstance() override; + void init(rclcpp::Node * node) override; - // void updateModuleParams(const std::vector & parameters) override; + std::unique_ptr createNewSceneModuleInstance() override; private: std::shared_ptr avoidance_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 4a117e5350baf..61caf7b2b393f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -14,12 +14,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__NORMAL_HPP_ -#include "behavior_path_planner/marker_utils/utils.hpp" #include "behavior_path_planner/scene_module/lane_change/base_class.hpp" #include -#include -#include #include #include @@ -36,8 +33,6 @@ using geometry_msgs::msg::Point; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using route_handler::Direction; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; class NormalLaneChange : public LaneChangeBase { @@ -96,6 +91,8 @@ class NormalLaneChange : public LaneChangeBase bool isLaneChangeRequired() const override; + bool isStoppedAtRedTrafficLight() const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp deleted file mode 100644 index 03672aa59f6b9..0000000000000 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_visitor.hpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_VISITOR_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_VISITOR_HPP_ - -#include "tier4_planning_msgs/msg/avoidance_debug_msg.hpp" -#include "tier4_planning_msgs/msg/avoidance_debug_msg_array.hpp" -#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/detail/lane_change_debug_msg_array__struct.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg.hpp" -#include "tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp" - -#include -namespace behavior_path_planner -{ -// Forward Declaration -class AvoidanceModule; -class AvoidanceByLCModule; -class LaneChangeModule; -class ExternalRequestLaneChangeModule; -class LaneChangeInterface; -class LaneChangeBTInterface; -class LaneFollowingModule; -class StartPlannerModule; -class GoalPlannerModule; -class SideShiftModule; - -using tier4_planning_msgs::msg::AvoidanceDebugMsg; -using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; -using tier4_planning_msgs::msg::LaneChangeDebugMsg; -using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; - -class SceneModuleVisitor -{ -public: - void visitLaneChangeModule(const LaneChangeModule * module) const; - void visitExternalRequestLaneChangeModule(const ExternalRequestLaneChangeModule * module) const; - void visitLaneChangeInterface(const LaneChangeInterface * interface) const; - void visitLaneChangeBTInterface(const LaneChangeBTInterface * module) const; - void visitAvoidanceModule(const AvoidanceModule * module) const; - void visitAvoidanceByLCModule(const AvoidanceByLCModule * module) const; - - std::shared_ptr getAvoidanceModuleDebugMsg() const; - std::shared_ptr getLaneChangeModuleDebugMsg() const; - -protected: - mutable std::shared_ptr lane_change_visitor_; - mutable std::shared_ptr ext_request_lane_change_visitor_; - mutable std::shared_ptr external_request_lane_change_bt_visitor_; - mutable std::shared_ptr avoidance_visitor_; -}; -} // namespace behavior_path_planner - -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_VISITOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp index b74be38c0faf9..53bac3914ce86 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -31,12 +31,15 @@ namespace behavior_path_planner class SideShiftModuleManager : public SceneModuleManagerInterface { public: - SideShiftModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + SideShiftModuleManager() : SceneModuleManagerInterface{"side_shift"} {} + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp index fce31b6db7369..014a0f403f9ec 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SIDE_SHIFT__SIDE_SHIFT_MODULE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/side_shift/side_shift_parameters.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -43,7 +43,9 @@ class SideShiftModule : public SceneModuleInterface SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); bool isExecutionRequested() const override; bool isExecutionReady() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp index 3ee376f4111f0..10fcbf81f8d45 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__MANAGER_HPP_ -#include "behavior_path_planner/scene_module/scene_module_manager_interface.hpp" #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" +#include "behavior_path_planner_common/interface/scene_module_manager_interface.hpp" #include @@ -30,12 +30,15 @@ namespace behavior_path_planner class StartPlannerModuleManager : public SceneModuleManagerInterface { public: - StartPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config); + StartPlannerModuleManager() : SceneModuleManagerInterface{"start_planner"} {} + + void init(rclcpp::Node * node) override; std::unique_ptr createNewSceneModuleInstance() override { - return std::make_unique(name_, *node_, parameters_, rtc_interface_ptr_map_); + return std::make_unique( + name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } void updateModuleParams(const std::vector & parameters) override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp index 54e13de85e8da..e3249d7ffd311 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp @@ -15,17 +15,17 @@ #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__START_PLANNER__START_PLANNER_MODULE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -79,7 +79,9 @@ class StartPlannerModule : public SceneModuleInterface StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map); + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map); void updateModuleParams(const std::any & parameters) override { @@ -127,8 +129,14 @@ class StartPlannerModule : public SceneModuleInterface bool canTransitIdleToRunningState() override; + /** + * @brief init member variables. + */ + void initVariables(); + void initializeSafetyCheckParameters(); + bool requiresDynamicObjectsCollisionDetection() const; bool receivedNewRoute() const; bool isModuleRunning() const; @@ -198,7 +206,8 @@ class StartPlannerModule : public SceneModuleInterface PredictedObjects filterStopObjectsInPullOutLanes( const lanelet::ConstLanelets & pull_out_lanes, const double velocity_threshold) const; bool hasFinishedPullOut() const; - bool isBackwardDrivingComplete() const; + bool hasFinishedBackwardDriving() const; + bool hasCollisionWithDynamicObjects() const; bool isStopped(); bool isStuck(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 60cf9b1244236..68c64ff9b7112 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__AVOIDANCE_MODULE_DATA_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include @@ -54,7 +54,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug; struct ObjectParameter { - bool is_target{false}; + bool is_avoidance_target{false}; + + bool is_safety_check_target{false}; size_t execute_num{1}; @@ -159,6 +161,7 @@ struct AvoidanceParameters double object_check_min_forward_distance{0.0}; double object_check_max_forward_distance{0.0}; double object_check_backward_distance{0.0}; + double object_check_yaw_deviation{0.0}; // if the distance between object and goal position is less than this parameter, the module ignore // the object. @@ -282,6 +285,9 @@ struct AvoidanceParameters // policy bool use_shorten_margin_immediately{false}; + // policy + std::string policy_approval{"per_shift_line"}; + // policy std::string policy_deceleration{"best_effort"}; @@ -394,8 +400,7 @@ struct ObjectData // avoidance target std::string reason{""}; // lateral avoid margin - // NOTE: If margin is less than the minimum margin threshold, boost::none will be set. - boost::optional avoid_margin{boost::none}; + std::optional avoid_margin{std::nullopt}; }; using ObjectDataArray = std::vector; @@ -414,10 +419,10 @@ struct AvoidLine : public ShiftLine double end_longitudinal = 0.0; // for unique_id - uint64_t id = 0; + UUID id{}; // for the case the point is created by merge other points - std::vector parent_ids{}; + std::vector parent_ids{}; // corresponding object ObjectData object{}; @@ -482,6 +487,7 @@ struct AvoidancePlanningData // current driving lanelet lanelet::ConstLanelets current_lanelets; + lanelet::ConstLanelets extend_lanelets; // output path ShiftedPath candidate_path; @@ -493,10 +499,7 @@ struct AvoidancePlanningData ObjectDataArray other_objects; // nearest object that should be avoid - boost::optional stop_target_object{boost::none}; - - // raw shift point - AvoidLineArray raw_shift_line{}; + std::optional stop_target_object{std::nullopt}; // new shift point AvoidLineArray new_shift_line{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index e1d51122e5c1b..54a38b828e987 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -241,12 +241,21 @@ class AvoidanceHelper getEgoSpeed(), target_velocity, a_now, a_lim, j_lim, -1.0 * j_lim); if (!!ret) { - return ret.get(); + return ret.value(); } return std::numeric_limits::max(); } + bool isComfortable(const AvoidLineArray & shift_lines) const + { + return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) { + return PathShifter::calcJerkFromLatLonDistance( + line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) < + getLateralMaxJerkLimit(); + }); + } + bool isShifted() const { return std::abs(getEgoShift()) > parameters_->lateral_avoid_check_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp new file mode 100644 index 0000000000000..054a6e15ef6bf --- /dev/null +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/shift_line_generator.hpp @@ -0,0 +1,251 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ +#define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ + +#include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" +#include "behavior_path_planner/utils/avoidance/helper.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" + +#include + +#include + +namespace behavior_path_planner::utils::avoidance +{ + +using behavior_path_planner::PathShifter; +using behavior_path_planner::helper::avoidance::AvoidanceHelper; +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; + +class ShiftLineGenerator +{ +public: + explicit ShiftLineGenerator(const std::shared_ptr & parameters) + : parameters_{parameters} + { + } + + void update(AvoidancePlanningData & data, DebugData & debug); + + void setRawRegisteredShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data); + + void setData(const std::shared_ptr & data) { data_ = data; } + + void setHelper(const std::shared_ptr & helper) { helper_ = helper; } + + void setPathShifter(const PathShifter & path_shifter) + { + base_offset_ = path_shifter.getBaseOffset(); + if (path_shifter.getShiftLines().empty()) { + last_ = std::nullopt; + } else { + last_ = path_shifter.getLastShiftLine().value(); + } + } + + void reset() + { + last_ = std::nullopt; + base_offset_ = 0.0; + raw_.clear(); + raw_registered_.clear(); + } + + AvoidLineArray generate(const AvoidancePlanningData & data, DebugData & debug) const; + + AvoidLineArray getRawRegisteredShiftLine() const { return raw_registered_; } + +private: + /** + * @brief Calculate the shift points (start/end point, shift length) from the object lateral + * and longitudinal positions in the Frenet coordinate. The jerk limit is also considered here. + * @param avoidance data. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines generateAvoidOutline(AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief merge avoid outlines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyMergeProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief fill gap between two shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidOutlines applyFillGapProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief generate candidate shift lines. + * @param one-shot shift lines. + * @param debug data. + */ + AvoidLineArray generateCandidateShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, + DebugData & debug) const; + + /** + * @brief clean up raw shift lines. + * @param target shift lines. + * @param debug data. + * @return processed shift lines. + * @details process flow: + * 1. combine raw shirt lines and previous registered shift lines. + * 2. add return shift line. + * 3. merge raw shirt lines. + * 4. trim unnecessary shift lines. + */ + AvoidLineArray applyPreProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const; + + /* + * @brief fill gap among shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidLineArray applyFillGapProcess( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, + DebugData & debug) const; + + /* + * @brief merge negative & positive shift lines. + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + */ + AvoidLineArray applyMergeProcess( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, + DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param current raw shift line. + * @param current registered shift line. + * @param debug data. + */ + AvoidLineArray applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + [[maybe_unused]] DebugData & debug) const; + + /* + * @brief add return shift line from ego position. + * @param shift lines which the return shift is added. + * Pick up the last shift point, which is the most farthest from ego, from the current candidate + * avoidance points and registered points in the shifter. If the last shift length of the point is + * non-zero, add a return-shift to center line from the point. If there is no shift point in + * candidate avoidance points nor registered points, and base_shift > 0, add a return-shift to + * center line from ego. + */ + AvoidLineArray addReturnShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, + DebugData & debug) const; + + /* + * @brief extract shift lines from total shift lines based on their gradient. + * @param shift length data. + * @return extracted shift lines. + */ + AvoidLineArray extractShiftLinesFromLine( + const AvoidancePlanningData & data, ShiftLineData & shift_line_data) const; + + /* + * @brief remove unnecessary avoid points + * @param original shift lines. + * @param debug data. + * @return processed shift lines. + * @details + * - Combine avoid points that have almost same gradient + * - Quantize the shift length to reduce the shift point noise + * - Change the shift length to the previous one if the deviation is small. + * - Remove unnecessary return shift (back to the center line). + */ + AvoidLineArray applyTrimProcess(const AvoidLineArray & shift_lines, DebugData & debug) const; + + /* + * @brief extract new shift lines based on current shifted path. the module makes a RTC request + * for those new shift lines. + * @param candidate shift lines. + * @return new shift lines. + */ + AvoidLineArray findNewShiftLine(const AvoidLineArray & shift_lines, DebugData & debug) const; + + /* + * @brief generate total shift line. total shift line has shift length and gradient array. + * @param raw shift lines. + * @param total shift line. + */ + void generateTotalShiftLine( + const AvoidLineArray & avoid_points, const AvoidancePlanningData & data, + ShiftLineData & shift_line_data) const; + + /* + * @brief quantize shift line length. + * @param target shift lines. + * @param threshold. shift length is quantized by this value. (if it is 0.3[m], the output shift + * length is 0.0, 0.3, 0.6...) + */ + void applyQuantizeProcess(AvoidLineArray & shift_lines, const double threshold) const; + + /* + * @brief trim shift line whose relative longitudinal distance is less than threshold. + * @param target shift lines. + * @param threshold. + */ + void applySmallShiftFilter(AvoidLineArray & shift_lines, const double threshold) const; + + /* + * @brief merge multiple shift lines whose relative gradient is less than threshold. + * @param target shift lines. + * @param threshold. + */ + void applySimilarGradFilter(AvoidLineArray & shift_lines, const double threshold) const; + + /** + * @brief update path index of the registered objects. remove old objects whose end point is + * behind ego pose. + */ + void updateRegisteredRawShiftLines(const AvoidancePlanningData & data); + + std::shared_ptr data_; + + std::shared_ptr parameters_; + + std::shared_ptr helper_; + + std::optional last_{std::nullopt}; + + AvoidLineArray raw_; + + AvoidLineArray raw_registered_; + + double base_offset_{0.0}; +}; + +} // namespace behavior_path_planner::utils::avoidance + +#endif // BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__SHIFT_LINE_GENERATOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp index 2e04935e37336..e7611208962f0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/utils.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__AVOIDANCE__UTILS_HPP_ -#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -33,18 +33,6 @@ using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygo bool isOnRight(const ObjectData & obj); -bool isVehicleTypeObject(const ObjectData & object); - -bool isWithinCrosswalk( - const ObjectData & object, - const std::shared_ptr & overall_graphs); - -bool isWithinIntersection( - const ObjectData & object, const std::shared_ptr & route_handler); - -bool isTargetObjectType( - const PredictedObject & object, const std::shared_ptr & parameters); - double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin); @@ -59,10 +47,9 @@ ShiftedPath toShiftedPath(const PathWithLaneId & path); ShiftLineArray toShiftLineArray(const AvoidLineArray & avoid_points); -std::vector concatParentIds( - const std::vector & ids1, const std::vector & ids2); +std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2); -std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2); +std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2); double lerpShiftLengthOnArc(double arc, const AvoidLine & al); @@ -106,9 +93,13 @@ lanelet::ConstLanelets getTargetLanelets( lanelet::ConstLanelets getCurrentLanesFromPath( const PathWithLaneId & path, const std::shared_ptr & planner_data); +lanelet::ConstLanelets getExtendLanes( + const lanelet::ConstLanelets & lanelets, const Pose & ego_pose, + const std::shared_ptr & planner_data); + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, - boost::optional & p_out); + std::optional & p_out); void fillObjectEnvelopePolygon( ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose, @@ -139,6 +130,11 @@ void filterTargetObjects( const std::shared_ptr & planner_data, const std::shared_ptr & parameters); +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters); + double extendToRoadShoulderDistanceWithPolygon( const std::shared_ptr & rh, const lanelet::ConstLineString3d & target_line, const double to_road_shoulder_distance, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp index f4ab10db61b9c..7f5eee7a12aaa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GEOMETRIC_PARALLEL_PARKING__GEOMETRIC_PARALLEL_PARKING_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/parameters.hpp" #include #include @@ -128,7 +128,7 @@ class GeometricParallelParking PathPointWithLaneId generateArcPathPoint( const Pose & center, const double radius, const double yaw, const bool is_left_turn, const bool is_forward); - boost::optional calcStartPose( + std::optional calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const double start_pose_offset, const double R_E_far, const bool is_forward, const bool left_side_parking); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp index 9cede67fff6e5..1595503a225f8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp @@ -33,6 +33,12 @@ class DefaultFixedGoalPlanner : public FixedGoalPlannerBase protected: PathWithLaneId modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const; + bool isPathValid( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) const; + lanelet::ConstLanelets extractLaneletsFromPath( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) const; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp index f587051442dcd..5fe3427749844 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/fixed_goal_planner_base.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__FIXED_GOAL_PLANNER_BASE_HPP_ -#include "behavior_path_planner/data_manager.hpp" +#include "behavior_path_planner_common/data_manager.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp index b698892907789..bb5d7b7555cfc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp @@ -41,7 +41,7 @@ class FreespacePullOver : public PullOverPlannerBase PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::FREESPACE; } - boost::optional plan(const Pose & goal_pose) override; + std::optional plan(const Pose & goal_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp index aee78930d2e1f..d96b01582a2d1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/geometric_pull_over.hpp @@ -45,7 +45,7 @@ class GeometricPullOver : public PullOverPlannerBase Pose getCr() const { return planner_.getCr(); } Pose getCl() const { return planner_.getCl(); } - boost::optional plan(const Pose & goal_pose) override; + std::optional plan(const Pose & goal_pose) override; std::vector generatePullOverPaths( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 2b189d583b924..c5adf3a2ad652 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -17,7 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp index 24c614e072b8f..7d1c99a69b1d5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp @@ -15,8 +15,8 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__GOAL_SEARCHER_BASE_HPP_ -#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp index 5023401263b8d..a0bfd8e883d80 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/pull_over_planner_base.hpp @@ -15,15 +15,13 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__PULL_OVER_PLANNER_BASE_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include #include -#include - #include #include #include @@ -127,7 +125,7 @@ class PullOverPlannerBase } virtual PullOverPlannerType getPlannerType() const = 0; - virtual boost::optional plan(const Pose & goal_pose) = 0; + virtual std::optional plan(const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp index dccaed3184e77..9247ec566fbd8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/shift_pull_over.hpp @@ -38,12 +38,12 @@ class ShiftPullOver : public PullOverPlannerBase const std::shared_ptr & occupancy_grid_map); PullOverPlannerType getPlannerType() const override { return PullOverPlannerType::SHIFT; }; - boost::optional plan(const Pose & goal_pose) override; + std::optional plan(const Pose & goal_pose) override; protected: PathWithLaneId generateReferencePath( const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; - boost::optional generatePullOverPath( + std::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const; static double calcBeforeShiftedArcLength( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp index 2977dab91fd14..25c0551d896b3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/util.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__GOAL_PLANNER__UTIL_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner/utils/goal_planner/goal_searcher_base.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 3f53f4b9c51e8..f11d48a24c79c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -15,7 +15,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp index f83c64192430c..c98f62c47860c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp @@ -15,9 +15,9 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__LANE_CHANGE_PATH_HPP_ -#include "behavior_path_planner/turn_signal_decider.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include @@ -42,8 +42,8 @@ struct LaneChangeStatus LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector lane_change_lane_ids{}; + std::vector lane_follow_lane_ids{}; + std::vector lane_change_lane_ids{}; bool is_safe{false}; bool is_valid_path{true}; double start_distance{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index ac712fa2f0fa5..12e6da00b2f36 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -15,11 +15,11 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include @@ -144,7 +144,7 @@ std::string getStrDirection(const std::string & name, const Direction direction) CandidateOutput assignToCandidate( const LaneChangePath & lane_change_path, const Point & ego_position); -boost::optional getLaneChangeTargetLane( +std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction); @@ -169,7 +169,7 @@ bool passParkedObject( const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, CollisionCheckDebugMap & object_debug); -boost::optional getLeadingStaticObjectIdx( +std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp index 89696190059cc..6475661f72ac4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/side_shift/side_shift_parameters.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__SIDE_SHIFT__SIDE_SHIFT_PARAMETERS_HPP_ -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp index 7a973a3bd699f..84d84e0628cad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__COMMON_MODULE_DATA_HPP_ -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp index 37206c79542ff..16be48b5acc6d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_goal_planner_common/utils.hpp @@ -15,12 +15,12 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_GOAL_PLANNER_COMMON__UTILS_HPP_ -#include "behavior_path_planner/data_manager.hpp" #include "behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/common_module_data.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include @@ -37,7 +37,7 @@ using behavior_path_planner::utils::path_safety_checker::EgoPredictedPathParams; using behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams; using behavior_path_planner::utils::path_safety_checker::SafetyCheckParams; -boost::optional calcFeasibleDecelDistance( +std::optional calcFeasibleDecelDistance( std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, const double target_velocity); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp index d78df16f89a80..3a154dfd83353 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/freespace_pull_out.hpp @@ -40,7 +40,7 @@ class FreespacePullOut : public PullOutPlannerBase PlannerType getPlannerType() override { return PlannerType::FREESPACE; } - boost::optional plan(const Pose & start_pose, const Pose & end_pose) override; + std::optional plan(const Pose & start_pose, const Pose & end_pose) override; protected: std::unique_ptr planner_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp index a5ff52e74038f..4f46350d6c2b1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/geometric_pull_out.hpp @@ -29,7 +29,7 @@ class GeometricPullOut : public PullOutPlannerBase explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; - boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; + std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp index 608b918e839fd..9c9c6fe83e288 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_path.hpp @@ -15,7 +15,7 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PATH_HPP_ -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp index 4c9271d57127d..60cd2731eb92f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/pull_out_planner_base.hpp @@ -15,16 +15,14 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__PULL_OUT_PLANNER_BASE_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/start_planner/pull_out_path.hpp" #include "behavior_path_planner/utils/start_planner/start_planner_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" #include #include -#include - #include namespace behavior_path_planner @@ -58,7 +56,7 @@ class PullOutPlannerBase } virtual PlannerType getPlannerType() = 0; - virtual boost::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; + virtual std::optional plan(const Pose & start_pose, const Pose & goal_pose) = 0; protected: std::shared_ptr planner_data_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp index 78a64feb2c1ec..7a868e6c2a234 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/shift_pull_out.hpp @@ -37,7 +37,7 @@ class ShiftPullOut : public PullOutPlannerBase std::shared_ptr & lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::SHIFT; }; - boost::optional plan(const Pose & start_pose, const Pose & goal_pose) override; + std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; std::vector calcPullOutPaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & road_lanes, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp index bd4017e5dff7d..46b72cacac651 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/start_planner_parameters.hpp @@ -17,7 +17,7 @@ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__START_PLANNER_PARAMETERS_HPP_ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -46,7 +46,6 @@ struct StartPlannerParameters double collision_check_distance_from_end{0.0}; double th_moving_object_velocity{0.0}; double center_line_path_interval{0.0}; - bool verbose{false}; // shift pull out bool enable_shift_pull_out{false}; @@ -93,6 +92,8 @@ struct StartPlannerParameters utils::path_safety_checker::EgoPredictedPathParams ego_predicted_path_params{}; utils::path_safety_checker::ObjectsFilteringParams objects_filtering_params{}; utils::path_safety_checker::SafetyCheckParams safety_check_params{}; + + bool print_debug_info{false}; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp index 71571b6d7a6c6..5fcb5fd5668b1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/start_planner/util.hpp @@ -15,10 +15,10 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ #define BEHAVIOR_PATH_PLANNER__UTILS__START_PLANNER__UTIL_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include diff --git a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml index d1dac8ad8d93e..19ea0491e776f 100644 --- a/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml +++ b/planning/behavior_path_planner/launch/behavior_path_planner.launch.xml @@ -6,17 +6,7 @@ - - - - - - - - - - - + @@ -48,17 +38,7 @@ - - - - - - - - - - - + diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 1f1eed3f95ec2..5ca86ee5a47b6 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -50,7 +50,7 @@ autoware_auto_tf2 autoware_auto_vehicle_msgs autoware_perception_msgs - behaviortree_cpp_v3 + behavior_path_planner_common freespace_planning_algorithms geometry_msgs interpolation @@ -62,10 +62,10 @@ motion_utils object_recognition_utils planning_test_utils + pluginlib rclcpp rclcpp_components route_handler - rtc_interface sensor_msgs signal_processing tf2 diff --git a/planning/behavior_path_planner/plugins.xml b/planning/behavior_path_planner/plugins.xml new file mode 100644 index 0000000000000..561246fcf75e4 --- /dev/null +++ b/planning/behavior_path_planner/plugins.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 689444a3bf995..907c5226c3908 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -14,16 +14,9 @@ #include "behavior_path_planner/behavior_path_planner_node.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp" -#include "behavior_path_planner/scene_module/goal_planner/manager.hpp" -#include "behavior_path_planner/scene_module/lane_change/interface.hpp" -#include "behavior_path_planner/scene_module/lane_change/manager.hpp" -#include "behavior_path_planner/scene_module/side_shift/manager.hpp" -#include "behavior_path_planner/scene_module/start_planner/manager.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -79,8 +72,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod create_publisher("~/output/is_reroute_available", 1); debug_avoidance_msg_array_publisher_ = create_publisher("~/debug/avoidance_debug_message_array", 1); - debug_lane_change_msg_array_publisher_ = - create_publisher("~/debug/lane_change_debug_message_array", 1); if (planner_data_->parameters.visualize_maximum_drivable_area) { debug_maximum_drivable_area_publisher_ = @@ -91,6 +82,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod bound_publisher_ = create_publisher("~/debug/bound", 1); + const auto qos_transient_local = rclcpp::QoS{1}.transient_local(); // subscriber velocity_subscriber_ = create_subscription( "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onOdometry, this, _1), @@ -115,7 +107,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "~/input/lateral_offset", 1, std::bind(&BehaviorPathPlannerNode::onLateralOffset, this, _1), createSubscriptionOptions(this)); operation_mode_subscriber_ = create_subscription( - "/system/operation_mode/state", 1, + "/system/operation_mode/state", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onOperationMode, this, _1), createSubscriptionOptions(this)); scenario_subscriber_ = create_subscription( @@ -126,7 +118,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod createSubscriptionOptions(this)); // route_handler - auto qos_transient_local = rclcpp::QoS{1}.transient_local(); vector_map_subscriber_ = create_subscription( "~/input/vector_map", qos_transient_local, std::bind(&BehaviorPathPlannerNode::onMap, this, _1), createSubscriptionOptions(this)); @@ -143,84 +134,15 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod const auto & p = planner_data_->parameters; planner_manager_ = std::make_shared(*this, p.max_iteration_num, p.verbose); - const auto register_and_create_publisher = - [&](const auto & manager, const bool create_publishers) { - const auto & module_name = manager->name(); - planner_manager_->registerSceneModuleManager(manager); - if (create_publishers) { - path_candidate_publishers_.emplace( - module_name, create_publisher(path_candidate_name_space + module_name, 1)); - path_reference_publishers_.emplace( - module_name, create_publisher(path_reference_name_space + module_name, 1)); - } - }; - - if (p.config_start_planner.enable_module) { - auto manager = - std::make_shared(this, "start_planner", p.config_start_planner); - register_and_create_publisher(manager, true); - } - - if (p.config_goal_planner.enable_module) { - auto manager = - std::make_shared(this, "goal_planner", p.config_goal_planner); - register_and_create_publisher(manager, true); - } - - if (p.config_side_shift.enable_module) { - auto manager = - std::make_shared(this, "side_shift", p.config_side_shift); - register_and_create_publisher(manager, true); - } - - if (p.config_lane_change_left.enable_module) { - const std::string module_topic = "lane_change_left"; - auto manager = std::make_shared( - this, module_topic, p.config_lane_change_left, route_handler::Direction::LEFT, - LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager, true); - } - - if (p.config_lane_change_right.enable_module) { - const std::string module_topic = "lane_change_right"; - auto manager = std::make_shared( - this, module_topic, p.config_lane_change_right, route_handler::Direction::RIGHT, - LaneChangeModuleType::NORMAL); - register_and_create_publisher(manager, true); - } - - if (p.config_ext_request_lane_change_right.enable_module) { - const std::string module_topic = "external_request_lane_change_right"; - auto manager = std::make_shared( - this, module_topic, p.config_ext_request_lane_change_right, route_handler::Direction::RIGHT, - LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager, true); + for (const auto & name : declare_parameter>("launch_modules")) { + planner_manager_->launchScenePlugin(*this, name); } - if (p.config_ext_request_lane_change_left.enable_module) { - const std::string module_topic = "external_request_lane_change_left"; - auto manager = std::make_shared( - this, module_topic, p.config_ext_request_lane_change_left, route_handler::Direction::LEFT, - LaneChangeModuleType::EXTERNAL_REQUEST); - register_and_create_publisher(manager, true); - } - - if (p.config_avoidance.enable_module) { - auto manager = - std::make_shared(this, "avoidance", p.config_avoidance); - register_and_create_publisher(manager, true); - } - - if (p.config_avoidance_by_lc.enable_module) { - auto manager = std::make_shared( - this, "avoidance_by_lane_change", p.config_avoidance_by_lc); - register_and_create_publisher(manager, true); - } - - if (p.config_dynamic_avoidance.enable_module) { - auto manager = std::make_shared( - this, "dynamic_avoidance", p.config_dynamic_avoidance); - register_and_create_publisher(manager, false); + for (const auto & manager : planner_manager_->getSceneModuleManagers()) { + path_candidate_publishers_.emplace( + manager->name(), create_publisher(path_candidate_name_space + manager->name(), 1)); + path_reference_publishers_.emplace( + manager->name(), create_publisher(path_reference_name_space + manager->name(), 1)); } } @@ -284,33 +206,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.max_iteration_num = declare_parameter("max_iteration_num"); p.traffic_light_signal_timeout = declare_parameter("traffic_light_signal_timeout"); - const auto get_scene_module_manager_param = [&](std::string && ns) { - ModuleConfigParameters config; - config.enable_module = declare_parameter(ns + "enable_module"); - config.enable_rtc = declare_parameter(ns + "enable_rtc"); - config.enable_simultaneous_execution_as_approved_module = - declare_parameter(ns + "enable_simultaneous_execution_as_approved_module"); - config.enable_simultaneous_execution_as_candidate_module = - declare_parameter(ns + "enable_simultaneous_execution_as_candidate_module"); - config.keep_last = declare_parameter(ns + "keep_last"); - config.priority = declare_parameter(ns + "priority"); - config.max_module_size = declare_parameter(ns + "max_module_size"); - return config; - }; - - p.config_start_planner = get_scene_module_manager_param("start_planner."); - p.config_goal_planner = get_scene_module_manager_param("goal_planner."); - p.config_side_shift = get_scene_module_manager_param("side_shift."); - p.config_lane_change_left = get_scene_module_manager_param("lane_change_left."); - p.config_lane_change_right = get_scene_module_manager_param("lane_change_right."); - p.config_ext_request_lane_change_right = - get_scene_module_manager_param("external_request_lane_change_right."); - p.config_ext_request_lane_change_left = - get_scene_module_manager_param("external_request_lane_change_left."); - p.config_avoidance = get_scene_module_manager_param("avoidance."); - p.config_avoidance_by_lc = get_scene_module_manager_param("avoidance_by_lc."); - p.config_dynamic_avoidance = get_scene_module_manager_param("dynamic_avoidance."); - // vehicle info const auto vehicle_info = VehicleInfoUtil(*this).getVehicleInfo(); p.vehicle_info = vehicle_info; @@ -513,7 +408,9 @@ void BehaviorPathPlannerNode::run() // Reset behavior tree when new route is received, // so that the each modules do not have to care about the "route jump". if (!is_first_time && !has_same_route_id) { + RCLCPP_INFO(get_logger(), "New uuid route is received. Resetting modules."); planner_manager_->reset(); + planner_data_->prev_modified_goal.reset(); } } @@ -647,25 +544,21 @@ void BehaviorPathPlannerNode::publish_steering_factor( steering_factor_interface_ptr_->updateSteeringFactor( {intersection_pose, intersection_pose}, {intersection_distance, intersection_distance}, - SteeringFactor::INTERSECTION, steering_factor_direction, steering_factor_state, ""); + PlanningBehavior::INTERSECTION, steering_factor_direction, steering_factor_state, ""); } else { steering_factor_interface_ptr_->clearSteeringFactors(); } steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now()); } -void BehaviorPathPlannerNode::publish_reroute_availability() +void BehaviorPathPlannerNode::publish_reroute_availability() const { - const bool has_approved_modules = planner_manager_->hasApprovedModules(); - const bool has_candidate_modules = planner_manager_->hasCandidateModules(); - - // In the current behavior path planner, we might get unexpected behavior when rerouting while - // modules other than lane follow are active. Therefore, rerouting will be allowed only when the - // lane follow module is running Note that if there is a approved module or candidate module, it - // means non-lane-following modules are runnning. + // In the current behavior path planner, we might encounter unexpected behavior when rerouting + // while modules other than lane following are active. If non-lane-following module except + // always-executable module is approved and running, rerouting will not be　possible. RerouteAvailability is_reroute_available; is_reroute_available.stamp = this->now(); - if (has_approved_modules || has_candidate_modules) { + if (planner_manager_->hasNonAlwaysExecutableApprovedModules()) { is_reroute_available.availability = false; } else { is_reroute_available.availability = true; @@ -785,11 +678,6 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg( if (avoidance_debug_message) { debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message); } - - const auto lane_change_debug_message = debug_messages_data_ptr->getLaneChangeModuleDebugMsg(); - if (lane_change_debug_message) { - debug_lane_change_msg_array_publisher_->publish(*lane_change_debug_message); - } } void BehaviorPathPlannerNode::publishPathCandidate( diff --git a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp index ce9cedc816a19..7587c3bb3663e 100644 --- a/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/avoidance/debug.cpp @@ -14,9 +14,12 @@ #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include +#include + +#include #include #include @@ -471,6 +474,131 @@ MarkerArray createOverhangFurthestLineStringMarkerArray( return msg; } + +MarkerArray createDebugMarkerArray( + const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) +{ + using marker_utils::createLaneletsAreaMarkerArray; + using marker_utils::createObjectsMarkerArray; + using marker_utils::createPathMarkerArray; + using marker_utils::createPolygonMarkerArray; + using marker_utils::createPoseMarkerArray; + using marker_utils::createShiftGradMarkerArray; + using marker_utils::createShiftLengthMarkerArray; + using marker_utils::createShiftLineMarkerArray; + using marker_utils::showPolygon; + using marker_utils::showPredictedPath; + using marker_utils::showSafetyCheckInfo; + using tier4_planning_msgs::msg::AvoidanceDebugFactor; + + const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); + MarkerArray msg; + + const auto & path = data.reference_path; + + const auto add = [&msg](const MarkerArray & added) { appendMarkerArray(added, &msg); }; + + const auto addAvoidLine = + [&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.05) { + add(createAvoidLineMarkerArray(al_arr, ns, r, g, b, w)); + }; + + const auto addShiftLine = + [&](const ShiftLineArray & sl_arr, const auto & ns, auto r, auto g, auto b, double w = 0.1) { + add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); + }; + + const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { + add(createOtherObjectsMarkerArray(objects, ns)); + }; + + const auto addShiftLength = + [&](const auto & shift_length, const auto & ns, auto r, auto g, auto b) { + add(createShiftLengthMarkerArray(shift_length, path, ns, r, g, b)); + }; + + const auto addShiftGrad = [&]( + const auto & shift_grad, const auto & shift_length, const auto & ns, + auto r, auto g, auto b) { + add(createShiftGradMarkerArray(shift_grad, shift_length, path, ns, r, g, b)); + }; + + // ignore objects + { + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); + addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); + addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); + addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); + addObjects(data.other_objects, std::string("MovingObject")); + addObjects(data.other_objects, std::string("CrosswalkUser")); + addObjects(data.other_objects, std::string("OutOfTargetArea")); + addObjects(data.other_objects, std::string("NotNeedAvoidance")); + addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); + addObjects(data.other_objects, std::string("TooNearToGoal")); + } + + // shift line pre-process + { + addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); + addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); + addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); + addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.2, 0.8, 0.4, 0.3); + addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.2, 1.0, 0.2, 0.3); + } + + // merge process + { + addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); + } + + // trimming process + { + addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); + addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); + } + + // registering process + { + addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); + addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); + } + + // safety check + { + add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); + add(showPredictedPath(debug.collision_check, "ego_predicted_path")); + add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); + } + + // shift length + { + addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); + addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); + addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); + } + + // shift grad + { + addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); + addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); + addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); + addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); + } + + // misc + { + add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); + add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); + add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); + add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); + add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); + } + + return msg; +} } // namespace marker_utils::avoidance_marker std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr) @@ -509,14 +637,16 @@ std::string toStrInfo(const behavior_path_planner::AvoidLineArray & ap_arr) } std::string toStrInfo(const behavior_path_planner::AvoidLine & ap) { + using tier4_autoware_utils::toHexString; + std::stringstream pids; for (const auto pid : ap.parent_ids) { - pids << pid << ", "; + pids << toHexString(pid) << ", "; } const auto & ps = ap.start.position; const auto & pe = ap.end.position; std::stringstream ss; - ss << "id = " << ap.id << ", shift length: " << ap.end_shift_length + ss << "id = " << toHexString(ap.id) << ", shift length: " << ap.end_shift_length << ", start_idx: " << ap.start_idx << ", end_idx: " << ap.end_idx << ", start_dist = " << ap.start_longitudinal << ", end_dist = " << ap.end_longitudinal << ", start_shift_length: " << ap.start_shift_length << ", start: (" << ps.x << ", " << ps.y diff --git a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp index a7604124ebe43..c2a640f989b50 100644 --- a/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_utils/lane_change/debug.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/colors.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/marker_utils/colors.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index 478b74c9051bb..c867bffc8e9bd 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -14,9 +14,9 @@ #include "behavior_path_planner/planner_manager.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/ros/debug_publisher.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" @@ -32,7 +32,8 @@ namespace behavior_path_planner { PlannerManager::PlannerManager( rclcpp::Node & node, const size_t max_iteration_num, const bool verbose) -: logger_(node.get_logger().get_child("planner_manager")), +: plugin_loader_("behavior_path_planner", "behavior_path_planner::SceneModuleManagerInterface"), + logger_(node.get_logger().get_child("planner_manager")), clock_(*node.get_clock()), max_iteration_num_{max_iteration_num}, verbose_{verbose} @@ -41,6 +42,46 @@ PlannerManager::PlannerManager( debug_publisher_ptr_ = std::make_unique(&node, "~/debug"); } +void PlannerManager::launchScenePlugin(rclcpp::Node & node, const std::string & name) +{ + if (plugin_loader_.isClassAvailable(name)) { + const auto plugin = plugin_loader_.createSharedInstance(name); + plugin->init(&node); + + // Check if the plugin is already registered. + for (const auto & running_plugin : manager_ptrs_) { + if (plugin->name() == running_plugin->name()) { + RCLCPP_WARN_STREAM(node.get_logger(), "The plugin '" << name << "' is already loaded."); + return; + } + } + + // register + manager_ptrs_.push_back(plugin); + processing_time_.emplace(plugin->name(), 0.0); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is loaded."); + } else { + RCLCPP_ERROR_STREAM(node.get_logger(), "The scene plugin '" << name << "' is not available."); + } +} + +void PlannerManager::removeScenePlugin(rclcpp::Node & node, const std::string & name) +{ + auto it = std::remove_if(manager_ptrs_.begin(), manager_ptrs_.end(), [&](const auto plugin) { + return plugin->name() == name; + }); + + if (it == manager_ptrs_.end()) { + RCLCPP_WARN_STREAM( + node.get_logger(), + "The scene plugin '" << name << "' is not found in the registered modules."); + } else { + manager_ptrs_.erase(it, manager_ptrs_.end()); + processing_time_.erase(name); + RCLCPP_INFO_STREAM(node.get_logger(), "The scene plugin '" << name << "' is unloaded."); + } +} + BehaviorModuleOutput PlannerManager::run(const std::shared_ptr & data) { resetProcessingTime(); @@ -222,10 +263,7 @@ std::vector PlannerManager::getRequestModules( // Condition 1: always executable module can be added regardless of the existence of other // modules, so skip checking the existence of other modules. // in other cases, need to check the existence of other modules and which module can be added. - const bool has_non_always_executable_module = std::any_of( - approved_module_ptrs_.begin(), approved_module_ptrs_.end(), - [this](const auto & m) { return !getManager(m)->isAlwaysExecutableModule(); }); - if (!manager_ptr->isAlwaysExecutableModule() && has_non_always_executable_module) { + if (!manager_ptr->isAlwaysExecutableModule() && hasNonAlwaysExecutableApprovedModules()) { // pairs of find_block_module and is_executable std::vector, std::function>> conditions; @@ -368,7 +406,7 @@ BehaviorModuleOutput PlannerManager::getReferencePath( std::max(p.backward_path_length, p.backward_path_length + extra_margin); const auto lanelet_sequence = route_handler->getLaneletSequence( - root_lanelet_.get(), pose, backward_length, std::numeric_limits::max()); + root_lanelet_.value(), pose, backward_length, std::numeric_limits::max()); lanelet::ConstLanelet closest_lane{}; if (lanelet::utils::query::getClosestLaneletWithConstrains( @@ -813,18 +851,18 @@ void PlannerManager::resetRootLanelet(const std::shared_ptr & data) // if root_lanelet is not route lanelets, reset root lanelet. // this can be caused by rerouting. const auto & route_handler = data->route_handler; - if (!route_handler->isRouteLanelet(root_lanelet_.get())) { + if (!route_handler->isRouteLanelet(root_lanelet_.value())) { root_lanelet_ = root_lanelet; return; } // check ego is in same lane - if (root_lanelet_.get().id() == root_lanelet.id()) { + if (root_lanelet_.value().id() == root_lanelet.id()) { return; } // check ego is in next lane - const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.get()); + const auto next_lanelets = route_handler->getRoutingGraphPtr()->following(root_lanelet_.value()); for (const auto & next : next_lanelets) { if (next.id() == root_lanelet.id()) { return; diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 1bd4fb3fef616..df93df75410e1 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -15,13 +15,13 @@ #include "behavior_path_planner/scene_module/avoidance/avoidance_module.hpp" #include "behavior_path_planner/marker_utils/avoidance/debug.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -90,56 +90,6 @@ bool isBestEffort(const std::string & policy) return policy == "best_effort"; } -AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) -{ - AvoidLine ret{}; - - ret.start_idx = line1.start_idx; - ret.start_shift_length = line1.start_shift_length; - ret.start_longitudinal = line1.start_longitudinal; - - ret.end_idx = line2.end_idx; - ret.end_shift_length = line2.end_shift_length; - ret.end_longitudinal = line2.end_longitudinal; - - ret.id = id; - ret.object = line1.object; - - return ret; -} - -AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const uint64_t id) -{ - AvoidLine ret{}; - - ret.start_idx = line1.end_idx; - ret.start_shift_length = line1.end_shift_length; - ret.start_longitudinal = line1.end_longitudinal; - - ret.end_idx = line2.start_idx; - ret.end_shift_length = line2.start_shift_length; - ret.end_longitudinal = line2.start_longitudinal; - - ret.id = id; - ret.object = line1.object; - - return ret; -} - -AvoidLineArray toArray(const AvoidOutlines & outlines) -{ - AvoidLineArray ret{}; - for (const auto & outline : outlines) { - ret.push_back(outline.avoid_line); - ret.push_back(outline.return_line); - - std::for_each( - outline.middle_lines.begin(), outline.middle_lines.end(), - [&ret](const auto & line) { ret.push_back(line); }); - } - return ret; -} - lanelet::BasicLineString3d toLineString3d(const std::vector & bound) { lanelet::BasicLineString3d ret{}; @@ -151,10 +101,13 @@ lanelet::BasicLineString3d toLineString3d(const std::vector & bound) AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + helper_{std::make_shared(parameters)}, parameters_{parameters}, - helper_{parameters} + generator_{parameters} { } @@ -192,12 +145,12 @@ bool AvoidanceModule::canTransitSuccessState() const auto & data = avoid_data_; // Change input lane. -> EXIT. - if (!isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + if (!isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { RCLCPP_WARN(getLogger(), "Previous module lane is updated. Exit."); return true; } - helper_.setPreviousDrivingLanes(data.current_lanelets); + helper_->setPreviousDrivingLanes(data.current_lanelets); // Reach input path end point. -> EXIT. { @@ -246,7 +199,7 @@ bool AvoidanceModule::canTransitSuccessState() // Be able to canceling avoidance path. -> EXIT. if (!has_avoidance_target) { - if (!helper_.isShifted() && parameters_->enable_cancel_maneuver) { + if (!helper_->isShifted() && parameters_->enable_cancel_maneuver) { RCLCPP_INFO(getLogger(), "No objects. Cancel avoidance path. Exit."); return true; } @@ -259,12 +212,15 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat { // reference pose data.reference_pose = - utils::getUnshiftedEgoPose(getEgoPose(), helper_.getPreviousSplineShiftPath()); + utils::getUnshiftedEgoPose(getEgoPose(), helper_->getPreviousSplineShiftPath()); // lanelet info data.current_lanelets = utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_); + data.extend_lanelets = + utils::avoidance::getExtendLanes(data.current_lanelets, getEgoPose(), planner_data_); + // expand drivable lanes std::for_each( data.current_lanelets.begin(), data.current_lanelets.end(), [&](const auto & lanelet) { @@ -283,7 +239,7 @@ void AvoidanceModule::fillFundamentalData(AvoidancePlanningData & data, DebugDat parameters_->use_intersection_areas, false)); // reference path - if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { data.reference_path_rough = extendBackwardLength(*getPreviousModuleOutput().path); } else { data.reference_path_rough = *getPreviousModuleOutput().path; @@ -338,10 +294,10 @@ void AvoidanceModule::fillAvoidanceTargetObjects( // Separate dynamic objects based on whether they are inside or outside of the expanded lanelets. const auto sparse_resample_path = utils::resamplePathWithSpline( - helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); + helper_->getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output); const auto [object_within_target_lane, object_outside_target_lane] = utils::avoidance::separateObjectsByPath( - sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(), + sparse_resample_path, planner_data_, data, parameters_, helper_->getForwardDetectionRange(), is_running, debug); for (const auto & object : object_outside_target_lane.objects) { @@ -360,7 +316,7 @@ void AvoidanceModule::fillAvoidanceTargetObjects( filterTargetObjects(objects, data, debug, planner_data_, parameters_); // Calculate the distance needed to safely decelerate the ego vehicle to a stop line. - const auto feasible_stop_distance = helper_.getFeasibleDecelDistance(0.0, false); + const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false); std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) { o.to_stop_line = calcDistanceToStopLine(o); fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_); @@ -437,7 +393,7 @@ ObjectData AvoidanceModule::createObjectData( bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. - if (helper_.isShifted()) { + if (helper_->isShifted()) { RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); return false; } @@ -448,7 +404,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); const auto to_shift_start_point = calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); - if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { + if (to_shift_start_point < helper_->getMinimumPrepareDistance()) { RCLCPP_DEBUG( getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not " @@ -468,7 +424,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto stopped_for_the_object = getEgoSpeed() < TH_STOP_SPEED && std::abs(data.to_stop_line) < TH_STOP_POSITION; - const auto id = data.stop_target_object.get().object.object_id; + const auto id = data.stop_target_object.value().object.object_id; const auto same_id_obj = std::find_if( ego_stopped_objects_.begin(), ego_stopped_objects_.end(), [&id](const auto & o) { return o.object.object_id == id; }); @@ -480,7 +436,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const // registered objects whom the ego stopped for at the moment of stopping. if (stopped_for_the_object) { - ego_stopped_objects_.push_back(data.stop_target_object.get()); + ego_stopped_objects_.push_back(data.stop_target_object.value()); } return true; @@ -491,24 +447,13 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de auto path_shifter = path_shifter_; /** - * STEP1: Generate avoid outlines. - * Basically, avoid outlines are generated per target objects. - */ - const auto outlines = generateAvoidOutline(data, debug); - - /** - * STEP2: Create rough shift lines. - */ - data.raw_shift_line = applyPreProcess(outlines, debug); - - /** - * STEP3: Create candidate shift lines. + * STEP1: Create candidate shift lines. * Merge rough shift lines, and extract new shift lines. */ - const auto processed_shift_lines = generateCandidateShiftLine(data.raw_shift_line, debug); + const auto processed_shift_lines = generator_.generate(data, debug); /** - * Step4: Validate new shift lines. + * Step2: Validate new shift lines. * Output new shift lines only when the avoidance path which is generated from them doesn't have * huge offset from ego. */ @@ -519,7 +464,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de data.found_avoidance_path = found_new_sl || registered; /** - * STEP5: Set new shift lines. + * STEP3: Set new shift lines. * If there are new shift points, these shift points are registered in path_shifter in order to * generate candidate avoidance path. */ @@ -528,7 +473,7 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de } /** - * STEP6: Generate avoidance path. + * STEP4: Generate avoidance path. */ ShiftedPath spline_shift_path = utils::avoidance::toShiftedPath(data.reference_path); const auto success_spline_path_generation = @@ -538,11 +483,11 @@ void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & de : utils::avoidance::toShiftedPath(data.reference_path); /** - * STEP7: Check avoidance path safety. + * STEP5: Check avoidance path safety. * For each target objects and the objects in adjacent lanes, * check that there is a certain amount of margin in the lateral and longitudinal direction. */ - data.comfortable = isComfortable(data.new_shift_line); + data.comfortable = helper_->isComfortable(data.new_shift_line); data.safe = isSafePath(data.candidate_path, debug); } @@ -575,1377 +520,180 @@ void AvoidanceModule::fillEgoStatus( data.stop_target_object = o; data.to_stop_line = o.to_stop_line; break; - } - } - - const auto can_yield_maneuver = canYieldManeuver(data); - - /** - * If the output path is locked by outside of this module, don't update output path. - */ - if (isOutputPathLocked()) { - data.safe_shift_line.clear(); - data.candidate_path = helper_.getPreviousSplineShiftPath(); - RCLCPP_DEBUG_THROTTLE( - getLogger(), *clock_, 500, "this module is locked now. keep current path."); - return; - } - - /** - * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation. - */ - if (data.safe) { - data.yield_required = false; - data.safe_shift_line = data.new_shift_line; - return; - } - - /** - * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if - * the shift line is unsafe. - */ - if (!parameters_->enable_yield_maneuver) { - data.yield_required = false; - data.safe_shift_line = data.new_shift_line; - return; - } - - /** - * TODO(Satoshi OTA) Think yield maneuver in the middle of avoidance. - * Even if it is determined that a yield is necessary, the yield maneuver is not executed - * if the avoidance has already been initiated. - */ - if (!can_yield_maneuver) { - data.safe = true; // overwrite safety judge. - data.yield_required = false; - data.safe_shift_line = data.new_shift_line; - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status."); - return; - } - - /** - * Transit yield maneuver. Clear shift lines and output yield path. - */ - { - data.yield_required = true; - data.safe_shift_line = data.new_shift_line; - } - - /** - * Even if data.avoid_required is false, the module cancels registered shift point when the - * approved avoidance path is not safe. - */ - const auto approved_path_exist = !path_shifter_.getShiftLines().empty(); - if (approved_path_exist) { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. canceling approved path..."); - return; - } - - /** - * If the avoidance path is not safe in situation where the ego should avoid object, the ego - * stops in front of the front object with the necessary distance to avoid the object. - */ - { - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. transit yield maneuver..."); - } -} - -void AvoidanceModule::fillDebugData( - const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const -{ - if (!data.stop_target_object) { - return; - } - - if (helper_.isShifted()) { - return; - } - - if (data.new_shift_line.empty()) { - return; - } - - const auto o_front = data.stop_target_object.get(); - const auto object_type = utils::getHighestProbLabel(o_front.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto & vehicle_width = planner_data_->parameters.vehicle_width; - - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - - const auto variable = helper_.getSharpAvoidanceDistance( - helper_.getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); - const auto constant = helper_.getNominalPrepareDistance() + - object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal; - const auto total_avoid_distance = variable + constant; - - dead_pose_ = calcLongitudinalOffsetPose( - data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); - - if (!dead_pose_) { - dead_pose_ = getPose(data.reference_path.points.front()); - } -} - -AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const -{ - if (data.yield_required) { - return AvoidanceState::YIELD; - } - - if (!data.avoid_required) { - return AvoidanceState::NOT_AVOID; - } - - if (!data.found_avoidance_path) { - return AvoidanceState::AVOID_PATH_NOT_READY; - } - - if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { - return AvoidanceState::AVOID_PATH_READY; - } - - return AvoidanceState::AVOID_EXECUTE; -} - -void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) -{ - if (parameters_->disable_path_update) { - return; - } - - insertPrepareVelocity(path); - - switch (data.state) { - case AvoidanceState::NOT_AVOID: { - break; - } - case AvoidanceState::YIELD: { - insertYieldVelocity(path); - insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; - } - case AvoidanceState::AVOID_PATH_NOT_READY: { - insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; - } - case AvoidanceState::AVOID_PATH_READY: { - insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); - break; - } - case AvoidanceState::AVOID_EXECUTE: { - insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); - break; - } - default: - throw std::domain_error("invalid behavior"); - } - - insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - - setStopReason(StopReason::AVOIDANCE, path.path); -} - -void AvoidanceModule::updateRegisteredRawShiftLines() -{ - const auto & data = avoid_data_; - - utils::avoidance::fillAdditionalInfoFromPoint(data, registered_raw_shift_lines_); - - AvoidLineArray avoid_lines; - - const auto has_large_offset = [this](const auto & s) { - constexpr double THRESHOLD = 0.1; - const auto ego_shift_length = helper_.getEgoLinearShift(); - - const auto start_to_ego_longitudinal = -1.0 * s.start_longitudinal; - - if (start_to_ego_longitudinal < 0.0) { - return false; - } - - const auto reg_shift_length = - s.getGradient() * start_to_ego_longitudinal + s.start_shift_length; - - return std::abs(ego_shift_length - reg_shift_length) > THRESHOLD; - }; - - const auto ego_idx = data.ego_closest_path_index; - - for (const auto & s : registered_raw_shift_lines_) { - // invalid - if (s.end_idx < ego_idx) { - continue; - } - - // invalid - if (has_large_offset(s)) { - continue; - } - - // valid - avoid_lines.push_back(s); - } - - DEBUG_PRINT( - "ego_closest_path_index = %lu, registered_raw_shift_lines_ size: %lu -> %lu", - data.ego_closest_path_index, registered_raw_shift_lines_.size(), avoid_lines.size()); - - printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_ (before)"); - printShiftLines(avoid_lines, "registered_raw_shift_lines_ (after)"); - - registered_raw_shift_lines_ = avoid_lines; - debug_data_.step1_registered_shift_line = registered_raw_shift_lines_; -} - -AvoidLineArray AvoidanceModule::applyPreProcess( - const AvoidOutlines & outlines, DebugData & debug) const -{ - AvoidOutlines processed_outlines = outlines; - - /** - * Step1: Rough merge process. - * Merge multiple avoid outlines. If an avoid outlines' return shift line conflicts other - * outline's avoid shift line, those avoid outlines are merged. - */ - processed_outlines = applyMergeProcess(processed_outlines, debug); - - /** - * Step2: Fill gap process. - * Create and add new shift line to avoid outline in order to fill gaps between avoid shift line - * and middle shift lines, return shift line and middle shift lines. - */ - processed_outlines = applyFillGapProcess(processed_outlines, debug); - - /** - * Step3: Convert to AvoidLineArray from AvoidOutlines. - */ - AvoidLineArray processed_raw_lines = toArray(processed_outlines); - - /** - * Step4: Combine process. - * Use all registered points. For the current points, if the similar one of the current - * points are already registered, will not use it. - */ - processed_raw_lines = - applyCombineProcess(processed_raw_lines, registered_raw_shift_lines_, debug); - - /* - * Step5: Add return shift line. - * Add return-to-center shift point from the last shift point, if needed. - * If there is no shift points, set return-to center shift from ego. - */ - processed_raw_lines = addReturnShiftLine(processed_raw_lines, debug); - - /* - * Step6: Fill gap process. - * Create and add new shift line to avoid lines. - */ - return applyFillGapProcess(processed_raw_lines, debug); -} - -AvoidLineArray AvoidanceModule::generateCandidateShiftLine( - const AvoidLineArray & shift_lines, DebugData & debug) const -{ - AvoidLineArray processed_shift_lines = shift_lines; - - /** - * Step1: Merge process. - * Merge positive shift avoid lines and negative shift avoid lines. - */ - processed_shift_lines = applyMergeProcess(processed_shift_lines, debug); - - /** - * Step2: Clean up process. - * Remove noisy shift line and concat same gradient shift lines. - */ - processed_shift_lines = applyTrimProcess(processed_shift_lines, debug); - - /** - * Step3: Extract new shift lines. - * Compare processed shift lines and registered shift lines in order to find new shift lines. - */ - return findNewShiftLine(processed_shift_lines, debug); -} - -void AvoidanceModule::registerRawShiftLines(const AvoidLineArray & future) -{ - if (future.empty()) { - RCLCPP_ERROR(getLogger(), "future is empty! return."); - return; - } - - const auto old_size = registered_raw_shift_lines_.size(); - - auto future_with_info = future; - utils::avoidance::fillAdditionalInfoFromPoint(avoid_data_, future_with_info); - printShiftLines(future_with_info, "future_with_info"); - printShiftLines(registered_raw_shift_lines_, "registered_raw_shift_lines_"); - printShiftLines(current_raw_shift_lines_, "current_raw_shift_lines_"); - - // sort by longitudinal - std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) { - return a.end_longitudinal < b.end_longitudinal; - }); - - // calc relative lateral length - future_with_info.front().start_shift_length = getCurrentBaseShift(); - for (size_t i = 1; i < future_with_info.size(); ++i) { - future_with_info.at(i).start_shift_length = future_with_info.at(i - 1).end_shift_length; - } - - const auto is_registered = [this](const auto id) { - const auto & r = registered_raw_shift_lines_; - return std::any_of(r.begin(), r.end(), [id](const auto & s) { return s.id == id; }); - }; - - const auto same_id_shift_line = [this](const auto id) { - const auto & r = current_raw_shift_lines_; - const auto itr = std::find_if(r.begin(), r.end(), [id](const auto & s) { return s.id == id; }); - if (itr != r.end()) { - return *itr; - } - throw std::logic_error("not found same id current raw shift line."); - }; - - for (const auto & s : future_with_info) { - if (s.parent_ids.empty()) { - RCLCPP_ERROR(getLogger(), "avoid line for path_shifter must have parent_id."); - } - - for (const auto id : s.parent_ids) { - if (is_registered(id)) { - continue; - } - - registered_raw_shift_lines_.push_back(same_id_shift_line(id)); - } - } - - DEBUG_PRINT("registered object size: %lu -> %lu", old_size, registered_raw_shift_lines_.size()); -} - -AvoidOutlines AvoidanceModule::generateAvoidOutline( - AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const -{ - // To be consistent with changes in the ego position, the current shift length is considered. - const auto current_ego_shift = helper_.getEgoShift(); - const auto & base_link2rear = planner_data_->parameters.base_link2rear; - - // Calculate feasible shift length - const auto get_shift_profile = - [&]( - auto & object, const auto & desire_shift_length) -> std::optional> { - // use each object param - const auto object_type = utils::getHighestProbLabel(object.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto is_object_on_right = utils::avoidance::isOnRight(object); - - // use absolute dist for return-to-center, relative dist from current for avoiding. - const auto avoiding_shift = desire_shift_length - current_ego_shift; - const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(avoiding_shift); - - // calculate remaining distance. - const auto prepare_distance = helper_.getNominalPrepareDistance(); - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto constant = object_parameter.safety_buffer_longitudinal + - additional_buffer_longitudinal + prepare_distance; - const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; - const auto remaining_distance = object.longitudinal - constant; - const auto avoidance_distance = - has_enough_distance ? nominal_avoid_distance : remaining_distance; - - // nominal case. avoidable. - if (has_enough_distance) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - if (!isBestEffort(parameters_->policy_lateral_margin)) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // ego already has enough positive shift. - const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; - if (is_object_on_right && has_enough_positive_shift) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // ego already has enough negative shift. - const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3; - if (!is_object_on_right && has_enough_negative_shift) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // don't relax shift length since it can stop in front of the object. - if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // the avoidance path is already approved - const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; - const auto is_approved = (helper_.getShift(object_pos) > 0.0 && is_object_on_right) || - (helper_.getShift(object_pos) < 0.0 && !is_object_on_right); - if (is_approved) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // prepare distance is not enough. unavoidable. - if (remaining_distance < 1e-3) { - object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; - return std::nullopt; - } - - // calculate lateral jerk. - const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( - avoiding_shift, remaining_distance, helper_.getAvoidanceEgoSpeed()); - - // relax lateral jerk limit. avoidable. - if (required_jerk < helper_.getLateralMaxJerkLimit()) { - return std::make_pair(desire_shift_length, avoidance_distance); - } - - // avoidance distance is not enough. unavoidable. - if (!isBestEffort(parameters_->policy_deceleration)) { - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; - } - - // output avoidance path under lateral jerk constraints. - const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( - remaining_distance, helper_.getLateralMaxJerkLimit(), helper_.getAvoidanceEgoSpeed()); - - if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { - object.reason = "LessThanExecutionThreshold"; - return std::nullopt; - } - - const auto feasible_shift_length = - desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift - : -1.0 * feasible_relative_shift_length + current_ego_shift; - - const auto infeasible = - std::abs(feasible_shift_length - object.overhang_dist) < - 0.5 * planner_data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; - if (infeasible) { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 1000, "feasible shift length is not enough to avoid. "); - object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; - return std::nullopt; - } - - { - RCLCPP_WARN_THROTTLE( - getLogger(), *clock_, 1000, "use feasible shift length. [original: (%.2f) actual: (%.2f)]", - std::abs(avoiding_shift), feasible_relative_shift_length); - } - - return std::make_pair(feasible_shift_length, avoidance_distance); - }; - - const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; - - const auto is_valid_shift_line = [](const auto & s) { - return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; - }; - - AvoidOutlines outlines; - for (auto & o : data.target_objects) { - if (!o.avoid_margin) { - o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; - if (o.avoid_required && is_forward_object(o)) { - break; - } else { - continue; - } - } - - const auto is_object_on_right = utils::avoidance::isOnRight(o); - const auto desire_shift_length = - helper_.getShiftLength(o, is_object_on_right, o.avoid_margin.get()); - if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { - o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; - if (o.avoid_required && is_forward_object(o)) { - break; - } else { - continue; - } - } - - // use each object param - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters_->object_parameters.at(object_type); - const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); - - if (!feasible_shift_profile.has_value()) { - if (o.avoid_required && is_forward_object(o)) { - break; - } else { - continue; - } - } - - // use absolute dist for return-to-center, relative dist from current for avoiding. - const auto feasible_return_distance = - helper_.getMaxAvoidanceDistance(feasible_shift_profile.value().first); - - AvoidLine al_avoid; - { - const auto & additional_buffer_longitudinal = - object_parameter.use_conservative_buffer_longitudinal - ? planner_data_->parameters.base_link2front - : 0.0; - const auto offset = - object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; - const auto to_shift_end = o.longitudinal - offset; - const auto path_front_to_ego = - avoid_data_.arclength_from_ego.at(avoid_data_.ego_closest_path_index); - - // start point (use previous linear shift length as start shift length.) - al_avoid.start_longitudinal = [&]() { - const auto nearest_avoid_distance = - std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3); - - if (data.to_start_point > to_shift_end) { - return nearest_avoid_distance; - } - - const auto minimum_avoid_distance = - helper_.getMinAvoidanceDistance(feasible_shift_profile.value().first - current_ego_shift); - const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); - - return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); - }(); - - al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( - avoid_data_.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); - al_avoid.start = avoid_data_.reference_path.points.at(al_avoid.start_idx).point.pose; - al_avoid.start_shift_length = helper_.getLinearShift(al_avoid.start.position); - - // end point - al_avoid.end_shift_length = feasible_shift_profile.value().first; - al_avoid.end_longitudinal = to_shift_end; - - // misc - al_avoid.id = getOriginalShiftLineUniqueId(); - al_avoid.object = o; - al_avoid.object_on_right = utils::avoidance::isOnRight(o); - } - - AvoidLine al_return; - { - const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; - const auto to_shift_start = o.longitudinal + offset; - - // start point - al_return.start_shift_length = feasible_shift_profile.value().first; - al_return.start_longitudinal = to_shift_start; - - // end point - al_return.end_longitudinal = [&]() { - if (data.to_return_point > to_shift_start) { - return std::clamp( - data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start); - } - - return to_shift_start + feasible_return_distance; - }(); - al_return.end_shift_length = 0.0; - - // misc - al_return.id = getOriginalShiftLineUniqueId(); - al_return.object = o; - al_return.object_on_right = utils::avoidance::isOnRight(o); - } - - if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { - outlines.emplace_back(al_avoid, al_return); - } else { - o.reason = "InvalidShiftLine"; - continue; - } - - o.is_avoidable = true; - } - - // debug - { - std::vector debug_info_array; - const auto append = [&](const auto & o) { - AvoidanceDebugMsg debug_info; - debug_info.object_id = toHexString(o.object.object_id); - debug_info.longitudinal_distance = o.longitudinal; - debug_info.lateral_distance_from_centerline = o.lateral; - debug_info.allow_avoidance = o.reason == ""; - debug_info.failed_reason = o.reason; - debug_info_array.push_back(debug_info); - }; - - for (const auto & o : data.target_objects) { - append(o); - } - - debug_avoidance_initializer_for_shift_line_.clear(); - debug_avoidance_initializer_for_shift_line_ = std::move(debug_info_array); - debug_avoidance_initializer_for_shift_line_time_ = clock_->now(); - } - - utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); - - debug.step1_current_shift_line = toArray(outlines); - - return outlines; -} - -void AvoidanceModule::generateTotalShiftLine( - const AvoidLineArray & avoid_lines, ShiftLineData & shift_line_data) const -{ - const auto & path = avoid_data_.reference_path; - const auto & arcs = avoid_data_.arclength_from_ego; - const auto N = path.points.size(); - - auto & sl = shift_line_data; - - sl.shift_line = std::vector(N, 0.0); - sl.shift_line_grad = std::vector(N, 0.0); - - sl.pos_shift_line = std::vector(N, 0.0); - sl.neg_shift_line = std::vector(N, 0.0); - - sl.pos_shift_line_grad = std::vector(N, 0.0); - sl.neg_shift_line_grad = std::vector(N, 0.0); - - // debug - sl.shift_line_history = std::vector>(avoid_lines.size(), sl.shift_line); - - // take minmax for same directional shift length - for (size_t j = 0; j < avoid_lines.size(); ++j) { - const auto & al = avoid_lines.at(j); - for (size_t i = 0; i < N; ++i) { - // calc current interpolated shift - const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al); - - // update maximum shift for positive direction - if (i_shift > sl.pos_shift_line.at(i)) { - sl.pos_shift_line.at(i) = i_shift; - sl.pos_shift_line_grad.at(i) = al.getGradient(); - } - - // update minumum shift for negative direction - if (i_shift < sl.neg_shift_line.at(i)) { - sl.neg_shift_line.at(i) = i_shift; - sl.neg_shift_line_grad.at(i) = al.getGradient(); - } - - // store for debug print - sl.shift_line_history.at(j).at(i) = i_shift; - } - } - - // Merge shift length of opposite directions. - for (size_t i = 0; i < N; ++i) { - sl.shift_line.at(i) = sl.pos_shift_line.at(i) + sl.neg_shift_line.at(i); - sl.shift_line_grad.at(i) = sl.pos_shift_line_grad.at(i) + sl.neg_shift_line_grad.at(i); - } - - // overwrite shift with current_ego_shift until ego pose. - const auto current_shift = helper_.getEgoLinearShift(); - for (size_t i = 0; i < sl.shift_line.size(); ++i) { - if (avoid_data_.ego_closest_path_index < i) { - break; - } - sl.shift_line.at(i) = current_shift; - sl.shift_line_grad.at(i) = 0.0; - } - - // If the shift point does not have an associated object, - // use previous value. - for (size_t i = 1; i < N; ++i) { - bool has_object = false; - for (const auto & al : avoid_lines) { - if (al.start_idx <= i && i <= al.end_idx) { - has_object = true; - break; - } - } - if (!has_object) { - sl.shift_line.at(i) = sl.shift_line.at(i - 1); - } - } - - if (avoid_lines.empty()) { - sl.shift_line_history.push_back(sl.shift_line); - return; - } - - const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / - avoid_lines.front().start_longitudinal; - - for (size_t i = avoid_data_.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { - sl.shift_line.at(i) = helper_.getLinearShift(getPoint(path.points.at(i))); - sl.shift_line_grad.at(i) = grad_first_shift_line; - } - - sl.shift_line_history.push_back(sl.shift_line); -} - -AvoidLineArray AvoidanceModule::extractShiftLinesFromLine(ShiftLineData & shift_line_data) const -{ - using utils::avoidance::setEndData; - using utils::avoidance::setStartData; - - const auto & path = avoid_data_.reference_path; - const auto & arcs = avoid_data_.arclength_from_ego; - const auto N = path.points.size(); - - auto & sl = shift_line_data; - - const auto backward_grad = [&](const size_t i) { - if (i == 0) { - return sl.shift_line_grad.at(i); - } - const double ds = arcs.at(i) - arcs.at(i - 1); - if (ds < 1.0e-5) { - return sl.shift_line_grad.at(i); - } // use theoretical value when ds is too small. - return (sl.shift_line.at(i) - sl.shift_line.at(i - 1)) / ds; - }; - - const auto forward_grad = [&](const size_t i) { - if (i == arcs.size() - 1) { - return sl.shift_line_grad.at(i); - } - const double ds = arcs.at(i + 1) - arcs.at(i); - if (ds < 1.0e-5) { - return sl.shift_line_grad.at(i); - } // use theoretical value when ds is too small. - return (sl.shift_line.at(i + 1) - sl.shift_line.at(i)) / ds; - }; - - // calculate forward and backward gradient of the shift length. - // This will be used for grad-change-point check. - sl.forward_grad = std::vector(N, 0.0); - sl.backward_grad = std::vector(N, 0.0); - for (size_t i = 0; i < N - 1; ++i) { - sl.forward_grad.at(i) = forward_grad(i); - sl.backward_grad.at(i) = backward_grad(i); - } - - AvoidLineArray merged_avoid_lines; - AvoidLine al{}; - bool found_first_start = false; - constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; - constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; - for (size_t i = avoid_data_.ego_closest_path_index; i < N - 1; ++i) { - const auto & p = path.points.at(i).point.pose; - const auto shift = sl.shift_line.at(i); - - // If the vehicle is already on the avoidance (checked by the first point has shift), - // set a start point at the first path point. - if (!found_first_start && std::abs(shift) > IS_ALREADY_SHIFTING_THR) { - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. - found_first_start = true; - DEBUG_PRINT("shift (= %f) is not zero at i = %lu. set start shift here.", shift, i); - } - - // find the point where the gradient of the shift is changed - const bool set_shift_line_flag = - std::abs(sl.forward_grad.at(i) - sl.backward_grad.at(i)) > CREATE_SHIFT_GRAD_THR; - - if (!set_shift_line_flag) { - continue; - } - - if (!found_first_start) { - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. - found_first_start = true; - DEBUG_PRINT("grad change detected. start at i = %lu", i); - } else { - setEndData(al, shift, p, i, arcs.at(i)); - al.id = getOriginalShiftLineUniqueId(); - merged_avoid_lines.push_back(al); - setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. - DEBUG_PRINT("end and start point found at i = %lu", i); - } - } - - if (std::abs(backward_grad(N - 1)) > CREATE_SHIFT_GRAD_THR) { - const auto & p = path.points.at(N - 1).point.pose; - const auto shift = sl.shift_line.at(N - 1); - setEndData(al, shift, p, N - 1, arcs.at(N - 1)); - al.id = getOriginalShiftLineUniqueId(); - merged_avoid_lines.push_back(al); - } - - return merged_avoid_lines; -} - -AvoidOutlines AvoidanceModule::applyMergeProcess( - const AvoidOutlines & outlines, DebugData & debug) const -{ - AvoidOutlines ret{}; - - if (outlines.size() < 2) { - return outlines; - } - - const auto no_conflict = [](const auto & line1, const auto & line2) { - return line1.end_idx < line2.start_idx || line2.end_idx < line1.start_idx; - }; - - const auto same_side_shift = [](const auto & line1, const auto & line2) { - return line1.object_on_right == line2.object_on_right; - }; - - const auto within = [](const auto & line, const size_t idx) { - return line.start_idx < idx && idx < line.end_idx; - }; - - ret.push_back(outlines.front()); - - for (size_t i = 1; i < outlines.size(); i++) { - auto & last_outline = ret.back(); - auto & next_outline = outlines.at(i); - - const auto & return_line = last_outline.return_line; - const auto & avoid_line = next_outline.avoid_line; - - if (no_conflict(return_line, avoid_line)) { - ret.push_back(outlines.at(i)); - continue; - } - - const auto merged_shift_line = merge(return_line, avoid_line, getOriginalShiftLineUniqueId()); - - if (!isComfortable(AvoidLineArray{merged_shift_line})) { - ret.push_back(outlines.at(i)); - continue; - } - - if (same_side_shift(return_line, avoid_line)) { - last_outline.middle_lines.push_back(merged_shift_line); - last_outline.return_line = next_outline.return_line; - debug.step1_merged_shift_line.push_back(merged_shift_line); - continue; - } - - if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { - last_outline.middle_lines.push_back(merged_shift_line); - last_outline.return_line = next_outline.return_line; - debug.step1_merged_shift_line.push_back(merged_shift_line); - continue; - } - - if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { - last_outline.middle_lines.push_back(merged_shift_line); - last_outline.return_line = next_outline.return_line; - debug.step1_merged_shift_line.push_back(merged_shift_line); - continue; - } - } - - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_merged_shift_line); - - return ret; -} - -AvoidOutlines AvoidanceModule::applyFillGapProcess( - const AvoidOutlines & outlines, DebugData & debug) const -{ - AvoidOutlines ret = outlines; - - for (auto & outline : ret) { - if (outline.middle_lines.empty()) { - const auto new_line = - fill(outline.avoid_line, outline.return_line, getOriginalShiftLineUniqueId()); - outline.middle_lines.push_back(new_line); - debug.step1_filled_shift_line.push_back(new_line); - } - - helper_.alignShiftLinesOrder(outline.middle_lines, false); - - if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { - const auto new_line = - fill(outline.avoid_line, outline.middle_lines.front(), getOriginalShiftLineUniqueId()); - outline.middle_lines.push_back(new_line); - debug.step1_filled_shift_line.push_back(new_line); - } - - helper_.alignShiftLinesOrder(outline.middle_lines, false); - - if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { - const auto new_line = - fill(outline.middle_lines.back(), outline.return_line, getOriginalShiftLineUniqueId()); - outline.middle_lines.push_back(new_line); - debug.step1_filled_shift_line.push_back(new_line); - } - - helper_.alignShiftLinesOrder(outline.middle_lines, false); - } - - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_filled_shift_line); - - return ret; -} - -AvoidLineArray AvoidanceModule::applyFillGapProcess( - const AvoidLineArray & shift_lines, DebugData & debug) const -{ - AvoidLineArray sorted = shift_lines; - - helper_.alignShiftLinesOrder(sorted, false); - - AvoidLineArray ret = sorted; - - if (shift_lines.empty()) { - return ret; - } - - const auto & data = avoid_data_; - - // fill gap between ego and nearest shift line. - if (sorted.front().start_longitudinal > 0.0) { - AvoidLine ego_line{}; - utils::avoidance::setEndData( - ego_line, helper_.getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, 0.0); - - const auto new_line = fill(ego_line, sorted.front(), getOriginalShiftLineUniqueId()); - ret.push_back(new_line); - debug.step1_front_shift_line.push_back(new_line); - } - - helper_.alignShiftLinesOrder(sorted, false); - - // fill gap among shift lines. - for (size_t i = 0; i < sorted.size() - 1; ++i) { - if (sorted.at(i + 1).start_longitudinal < sorted.at(i).end_longitudinal) { - continue; - } - - const auto new_line = fill(sorted.at(i), sorted.at(i + 1), getOriginalShiftLineUniqueId()); - ret.push_back(new_line); - debug.step1_front_shift_line.push_back(new_line); - } - - helper_.alignShiftLinesOrder(ret, false); - - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, ret); - utils::avoidance::fillAdditionalInfoFromLongitudinal(avoid_data_, debug.step1_front_shift_line); - - return ret; -} - -AvoidLineArray AvoidanceModule::applyCombineProcess( - const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, - DebugData & debug) const -{ - debug.step1_registered_shift_line = registered_lines; - return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); -} - -AvoidLineArray AvoidanceModule::applyMergeProcess( - const AvoidLineArray & shift_lines, DebugData & debug) const -{ - // Generate shift line by merging shift_lines. - ShiftLineData shift_line_data; - generateTotalShiftLine(shift_lines, shift_line_data); - - // Re-generate shift points by detecting gradient-change point of the shift line. - auto merged_shift_lines = extractShiftLinesFromLine(shift_line_data); - - // set parent id - for (auto & al : merged_shift_lines) { - al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); - } - - // sort by distance from ego. - helper_.alignShiftLinesOrder(merged_shift_lines); - - // debug visualize - { - debug.pos_shift = shift_line_data.pos_shift_line; - debug.neg_shift = shift_line_data.neg_shift_line; - debug.total_shift = shift_line_data.shift_line; - debug.pos_shift_grad = shift_line_data.pos_shift_line_grad; - debug.neg_shift_grad = shift_line_data.neg_shift_line_grad; - debug.total_forward_grad = shift_line_data.forward_grad; - debug.total_backward_grad = shift_line_data.backward_grad; - debug.step2_merged_shift_line = merged_shift_lines; - } - - return merged_shift_lines; -} - -AvoidLineArray AvoidanceModule::applyTrimProcess( - const AvoidLineArray & shift_lines, DebugData & debug) const -{ - if (shift_lines.empty()) { - return shift_lines; + } } - AvoidLineArray sl_array_trimmed = shift_lines; - - // sort shift points from front to back. - helper_.alignShiftLinesOrder(sl_array_trimmed); - - // - Change the shift length to the previous one if the deviation is small. - { - constexpr double SHIFT_DIFF_THRES = 1.0; - applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); - } + const auto can_yield_maneuver = canYieldManeuver(data); - // - Combine avoid points that have almost same gradient. - // this is to remove the noise. - { - const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; - applySimilarGradFilter(sl_array_trimmed, THRESHOLD); - debug.step3_grad_filtered_1st = sl_array_trimmed; + /** + * If the output path is locked by outside of this module, don't update output path. + */ + if (isOutputPathLocked()) { + data.safe_shift_line.clear(); + data.candidate_path = helper_->getPreviousSplineShiftPath(); + RCLCPP_DEBUG_THROTTLE( + getLogger(), *clock_, 500, "this module is locked now. keep current path."); + return; } - // - Quantize the shift length to reduce the shift point noise - // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. - { - const auto THRESHOLD = parameters_->quantize_filter_threshold; - applyQuantizeProcess(sl_array_trimmed, THRESHOLD); - debug.step3_quantize_filtered = sl_array_trimmed; + /** + * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation. + */ + if (data.safe) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + return; } - // - Change the shift length to the previous one if the deviation is small. - { - constexpr double SHIFT_DIFF_THRES = 1.0; - applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); - debug.step3_noise_filtered = sl_array_trimmed; + /** + * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if + * the shift line is unsafe. + */ + if (!parameters_->enable_yield_maneuver) { + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + return; } - // - Combine avoid points that have almost same gradient (again) - { - const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; - applySimilarGradFilter(sl_array_trimmed, THRESHOLD); - debug.step3_grad_filtered_2nd = sl_array_trimmed; + /** + * TODO(Satoshi OTA) Think yield maneuver in the middle of avoidance. + * Even if it is determined that a yield is necessary, the yield maneuver is not executed + * if the avoidance has already been initiated. + */ + if (!can_yield_maneuver) { + data.safe = true; // overwrite safety judge. + data.yield_required = false; + data.safe_shift_line = data.new_shift_line; + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. but could not transit yield status."); + return; } - // - Combine avoid points that have almost same gradient (again) + /** + * Transit yield maneuver. Clear shift lines and output yield path. + */ { - const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; - applySimilarGradFilter(sl_array_trimmed, THRESHOLD); - debug.step3_grad_filtered_3rd = sl_array_trimmed; + data.yield_required = true; + data.safe_shift_line = data.new_shift_line; } - return sl_array_trimmed; -} - -void AvoidanceModule::applyQuantizeProcess( - AvoidLineArray & shift_lines, const double threshold) const -{ - if (threshold < 1.0e-5) { - return; // no need to process + /** + * Even if data.avoid_required is false, the module cancels registered shift point when the + * approved avoidance path is not safe. + */ + const auto approved_path_exist = !path_shifter_.getShiftLines().empty(); + if (approved_path_exist) { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. canceling approved path..."); + return; } - for (auto & sl : shift_lines) { - sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; + /** + * If the avoidance path is not safe in situation where the ego should avoid object, the ego + * stops in front of the front object with the necessary distance to avoid the object. + */ + { + RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe. transit yield maneuver..."); } - - helper_.alignShiftLinesOrder(shift_lines); } -void AvoidanceModule::applySmallShiftFilter( - AvoidLineArray & shift_lines, const double threshold) const +void AvoidanceModule::fillDebugData( + const AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const { - if (shift_lines.empty()) { + if (!data.stop_target_object) { return; } - AvoidLineArray input = shift_lines; - shift_lines.clear(); - - for (const auto & s : input) { - if (s.getRelativeLongitudinal() < threshold) { - continue; - } - - if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) { - continue; - } - - shift_lines.push_back(s); + if (helper_->isShifted()) { + return; } -} -void AvoidanceModule::applySimilarGradFilter( - AvoidLineArray & avoid_lines, const double threshold) const -{ - if (avoid_lines.empty()) { + if (data.new_shift_line.empty()) { return; } - AvoidLineArray input = avoid_lines; - avoid_lines.clear(); - avoid_lines.push_back(input.front()); // Take the first one anyway (think later) - - AvoidLine base_line = input.front(); - - AvoidLineArray combine_buffer; - combine_buffer.push_back(input.front()); - - constexpr auto SHIFT_THR = 1e-3; - const auto is_negative_shift = [&](const auto & s) { - return s.getRelativeLength() < -1.0 * SHIFT_THR; - }; - - const auto is_positive_shift = [&](const auto & s) { return s.getRelativeLength() > SHIFT_THR; }; - - for (size_t i = 1; i < input.size(); ++i) { - AvoidLine combine{}; - - utils::avoidance::setStartData( - combine, base_line.start_shift_length, base_line.start, base_line.start_idx, - base_line.start_longitudinal); - utils::avoidance::setEndData( - combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, - input.at(i).end_longitudinal); - - combine_buffer.push_back(input.at(i)); + const auto o_front = data.stop_target_object.value(); + const auto object_type = utils::getHighestProbLabel(o_front.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal + ? planner_data_->parameters.base_link2front + : 0.0; + const auto & vehicle_width = planner_data_->parameters.vehicle_width; - const auto violates = [&]() { - if (is_negative_shift(input.at(i)) && is_positive_shift(base_line)) { - return true; - } + const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - if (is_negative_shift(base_line) && is_positive_shift(input.at(i))) { - return true; - } + const auto variable = helper_->getSharpAvoidanceDistance( + helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin)); + const auto constant = helper_->getNominalPrepareDistance() + + object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal; + const auto total_avoid_distance = variable + constant; - const auto lon_combine = combine.getRelativeLongitudinal(); - const auto base_length = base_line.getGradient() * lon_combine; - return std::abs(combine.getRelativeLength() - base_length) > threshold; - }(); + dead_pose_ = calcLongitudinalOffsetPose( + data.reference_path.points, getEgoPosition(), o_front.longitudinal - total_avoid_distance); - if (violates) { - avoid_lines.push_back(input.at(i)); - base_line = input.at(i); - combine_buffer.clear(); - combine_buffer.push_back(input.at(i)); - } else { - avoid_lines.back() = combine; - } + if (!dead_pose_) { + dead_pose_ = getPose(data.reference_path.points.front()); } - - helper_.alignShiftLinesOrder(avoid_lines); - - DEBUG_PRINT("size %lu -> %lu", input.size(), avoid_lines.size()); } -AvoidLineArray AvoidanceModule::addReturnShiftLine( - const AvoidLineArray & shift_lines, DebugData & debug) const +AvoidanceState AvoidanceModule::updateEgoState(const AvoidancePlanningData & data) const { - AvoidLineArray ret = shift_lines; - - constexpr double ep = 1.0e-3; - const auto & data = avoid_data_; - const bool has_candidate_point = !ret.empty(); - const bool has_registered_point = !path_shifter_.getShiftLines().empty(); - - const auto exist_unavoidable_object = std::any_of( - data.target_objects.begin(), data.target_objects.end(), - [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); + if (data.yield_required) { + return AvoidanceState::YIELD; + } - if (exist_unavoidable_object) { - return ret; + if (!data.avoid_required) { + return AvoidanceState::NOT_AVOID; } - // If the return-to-center shift points are already registered, do nothing. - if (!has_registered_point && std::fabs(getCurrentBaseShift()) < ep) { - DEBUG_PRINT("No shift points, not base offset. Do not have to add return-shift."); - return ret; + if (!data.found_avoidance_path) { + return AvoidanceState::AVOID_PATH_NOT_READY; } - constexpr double RETURN_SHIFT_THRESHOLD = 0.1; - DEBUG_PRINT("registered last shift = %f", path_shifter_.getLastShiftLength()); - if (std::abs(path_shifter_.getLastShiftLength()) < RETURN_SHIFT_THRESHOLD) { - DEBUG_PRINT("Return shift is already registered. do nothing."); - return ret; + if (isWaitingApproval() && path_shifter_.getShiftLines().empty()) { + return AvoidanceState::AVOID_PATH_READY; } - // From here, the return-to-center is not registered. But perhaps the candidate is - // already generated. + return AvoidanceState::AVOID_EXECUTE; +} - // If it has a shift point, add return shift from the existing last shift point. - // If not, add return shift from ego point. (prepare distance is considered for both.) - ShiftLine last_sl; // the return-shift will be generated after the last shift point. - { - // avoidance points: Yes, shift points: No -> select last avoidance point. - if (has_candidate_point && !has_registered_point) { - helper_.alignShiftLinesOrder(ret, false); - last_sl = ret.back(); - } +void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, ShiftedPath & path) +{ + if (parameters_->disable_path_update) { + return; + } - // avoidance points: No, shift points: Yes -> select last shift point. - if (!has_candidate_point && has_registered_point) { - last_sl = utils::avoidance::fillAdditionalInfo( - data, AvoidLine{path_shifter_.getLastShiftLine().get()}); - } + insertPrepareVelocity(path); - // avoidance points: Yes, shift points: Yes -> select the last one from both. - if (has_candidate_point && has_registered_point) { - helper_.alignShiftLinesOrder(ret, false); - const auto & al = ret.back(); - const auto & sl = utils::avoidance::fillAdditionalInfo( - data, AvoidLine{path_shifter_.getLastShiftLine().get()}); - last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; + switch (data.state) { + case AvoidanceState::NOT_AVOID: { + break; } - - // avoidance points: No, shift points: No -> set the ego position to the last shift point - // so that the return-shift will be generated from ego position. - if (!has_candidate_point && !has_registered_point) { - last_sl.end_idx = avoid_data_.ego_closest_path_index; - last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; - last_sl.end_shift_length = getCurrentBaseShift(); + case AvoidanceState::YIELD: { + insertYieldVelocity(path); + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); + break; } - } - - // There already is a shift point candidates to go back to center line, but it could be too sharp - // due to detection noise or timing. - // Here the return-shift from ego is added for the in case. - if (std::fabs(last_sl.end_shift_length) < RETURN_SHIFT_THRESHOLD) { - const auto current_base_shift = helper_.getEgoShift(); - if (std::abs(current_base_shift) < ep) { - DEBUG_PRINT("last shift almost is zero, and current base_shift is zero. do nothing."); - return ret; + case AvoidanceState::AVOID_PATH_NOT_READY: { + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); + break; } - - // Is there a shift point in the opposite direction of the current_base_shift? - // No -> we can overwrite the return shift, because the other shift points that decrease - // the shift length are for return-shift. - // Yes -> we can NOT overwrite, because it might be not a return-shift, but a avoiding - // shift to the opposite direction which can not be overwritten by the return-shift. - for (const auto & sl : ret) { - if ( - (current_base_shift > 0.0 && sl.end_shift_length < -ep) || - (current_base_shift < 0.0 && sl.end_shift_length > ep)) { - DEBUG_PRINT( - "try to put overwrite return shift, but there is shift for opposite direction. Skip " - "adding return shift."); - return ret; - } + case AvoidanceState::AVOID_PATH_READY: { + insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path); + break; } - - // If return shift already exists in candidate or registered shift lines, skip adding return - // shift. - if (has_candidate_point || has_registered_point) { - return ret; + case AvoidanceState::AVOID_EXECUTE: { + insertStopPoint(isBestEffort(parameters_->policy_deceleration), path); + break; } - - // set the return-shift from ego. - DEBUG_PRINT( - "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end_idx = avoid_data_.ego_closest_path_index; - last_sl.end = avoid_data_.reference_path.points.at(last_sl.end_idx).point.pose; - last_sl.end_shift_length = current_base_shift; - } - - const auto & arclength_from_ego = avoid_data_.arclength_from_ego; - - const auto nominal_prepare_distance = helper_.getNominalPrepareDistance(); - const auto nominal_avoid_distance = helper_.getMaxAvoidanceDistance(last_sl.end_shift_length); - - if (arclength_from_ego.empty()) { - return ret; - } - - const auto remaining_distance = std::min( - arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, - avoid_data_.to_return_point); - - // If the avoidance point has already been set, the return shift must be set after the point. - const auto last_sl_distance = avoid_data_.arclength_from_ego.at(last_sl.end_idx); - - // check if there is enough distance for return. - if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) - RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 1000, "No enough distance for return."); - return ret; + default: + throw std::domain_error("invalid behavior"); } - // If the remaining distance is not enough, the return shift needs to be shrunk. - // (or another option is just to ignore the return-shift.) - // But we do not want to change the last shift point, so we will shrink the distance after - // the last shift point. - // - // The line "===" is fixed, "---" is scaled. - // - // [Before Scaling] - // ego last_sl_end prepare_end path_end avoid_end - // ==o====================o----------------------o----------------------o------------o - // | prepare_dist | avoid_dist | - // - // [After Scaling] - // ==o====================o------------------o--------------------------o - // | prepare_dist_scaled | avoid_dist_scaled | - // - const double variable_prepare_distance = - std::max(nominal_prepare_distance - last_sl_distance, 0.0); - - double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); - double avoid_distance_scaled = nominal_avoid_distance; - if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { - const auto scale = (remaining_distance - last_sl_distance) / - std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); - prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; - avoid_distance_scaled *= scale; - DEBUG_PRINT( - "last_sl_distance = %f, nominal_prepare_distance = %f, nominal_avoid_distance = %f, " - "remaining_distance = %f, variable_prepare_distance = %f, scale = %f, " - "prepare_distance_scaled = %f,avoid_distance_scaled = %f", - last_sl_distance, nominal_prepare_distance, nominal_avoid_distance, remaining_distance, - variable_prepare_distance, scale, prepare_distance_scaled, avoid_distance_scaled); - } else { - DEBUG_PRINT("there is enough distance. Use nominal for prepare & avoidance."); - } - - // shift point for prepare distance: from last shift to return-start point. - if (nominal_prepare_distance > last_sl_distance) { - AvoidLine al; - al.id = getOriginalShiftLineUniqueId(); - al.start_idx = last_sl.end_idx; - al.start = last_sl.end; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); - al.end_shift_length = last_sl.end_shift_length; - al.start_shift_length = last_sl.end_shift_length; - ret.push_back(al); - debug.step1_return_shift_line.push_back(al); - } - - // shift point for return to center line - { - AvoidLine al; - al.id = getOriginalShiftLineUniqueId(); - al.start_idx = - utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); - al.start = avoid_data_.reference_path.points.at(al.start_idx).point.pose; - al.start_longitudinal = arclength_from_ego.at(al.start_idx); - al.end_idx = utils::avoidance::findPathIndexFromArclength( - arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); - al.end = avoid_data_.reference_path.points.at(al.end_idx).point.pose; - al.end_longitudinal = arclength_from_ego.at(al.end_idx); - al.end_shift_length = 0.0; - al.start_shift_length = last_sl.end_shift_length; - ret.push_back(al); - debug.step1_return_shift_line.push_back(al); - } + insertReturnDeadLine(isBestEffort(parameters_->policy_deceleration), path); - return ret; + setStopReason(StopReason::AVOIDANCE, path.path); } bool AvoidanceModule::isSafePath( @@ -1996,7 +744,7 @@ bool AvoidanceModule::isSafePath( avoid_data_, planner_data_, parameters_, is_right_shift.value()); for (const auto & object : safety_check_target_objects) { - auto current_debug_data = marker_utils::createObjectDebug(object); + auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); const auto obj_polygon = tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); @@ -2025,14 +773,15 @@ bool AvoidanceModule::isSafePath( if (!utils::path_safety_checker::checkCollision( shifted_path.path, ego_predicted_path, object, obj_path, p, parameters_->rss_params, hysteresis_factor, current_debug_data.second)) { - marker_utils::updateCollisionCheckDebugMap( + utils::path_safety_checker::updateCollisionCheckDebugMap( debug.collision_check, current_debug_data, false); safe_count_ = 0; return false; } } - marker_utils::updateCollisionCheckDebugMap(debug.collision_check, current_debug_data, true); + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug.collision_check, current_debug_data, true); } safe_count_++; @@ -2042,7 +791,7 @@ bool AvoidanceModule::isSafePath( PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & original_path) const { - const auto previous_path = helper_.getPreviousReferencePath(); + const auto previous_path = helper_->getPreviousReferencePath(); const auto longest_dist_to_shift_point = [&]() { double max_dist = 0.0; @@ -2050,7 +799,7 @@ PathWithLaneId AvoidanceModule::extendBackwardLength(const PathWithLaneId & orig max_dist = std::max( max_dist, calcSignedArcLength(previous_path.points, pnt.start.position, getEgoPosition())); } - for (const auto & sp : registered_raw_shift_lines_) { + for (const auto & sp : generator_.getRawRegisteredShiftLine()) { max_dist = std::max( max_dist, calcSignedArcLength(previous_path.points, sp.start.position, getEgoPosition())); } @@ -2122,11 +871,11 @@ BehaviorModuleOutput AvoidanceModule::plan() // set previous data if (success_spline_path_generation && success_linear_path_generation) { - helper_.setPreviousLinearShiftPath(linear_shift_path); - helper_.setPreviousSplineShiftPath(spline_shift_path); - helper_.setPreviousReferencePath(data.reference_path); + helper_->setPreviousLinearShiftPath(linear_shift_path); + helper_->setPreviousSplineShiftPath(spline_shift_path); + helper_->setPreviousReferencePath(data.reference_path); } else { - spline_shift_path = helper_.getPreviousSplineShiftPath(); + spline_shift_path = helper_->getPreviousSplineShiftPath(); } // post processing @@ -2162,7 +911,7 @@ BehaviorModuleOutput AvoidanceModule::plan() updateDebugMarker(avoid_data_, path_shifter_, debug_data_); } - if (isDrivingSameLane(helper_.getPreviousDrivingLanes(), data.current_lanelets)) { + if (isDrivingSameLane(helper_->getPreviousDrivingLanes(), data.current_lanelets)) { output.path = std::make_shared(spline_shift_path.path); } else { output.path = getPreviousModuleOutput().path; @@ -2216,14 +965,14 @@ CandidateOutput AvoidanceModule::planCandidate() const return output; } - const auto sl = helper_.getMainShiftLine(data.safe_shift_line); + const auto sl = helper_->getMainShiftLine(data.safe_shift_line); const auto sl_front = data.safe_shift_line.front(); const auto sl_back = data.safe_shift_line.back(); utils::clipPathLength( shifted_path.path, sl_front.start_idx, std::numeric_limits::max(), 0.0); - output.lateral_shift = helper_.getRelativeShiftToPath(sl); + output.lateral_shift = helper_->getRelativeShiftToPath(sl); output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; @@ -2233,8 +982,7 @@ CandidateOutput AvoidanceModule::planCandidate() const steering_factor_interface_ptr_->updateSteeringFactor( {sl_front.start, sl_back.end}, {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, - ""); + PlanningBehavior::AVOIDANCE, steering_factor_direction, SteeringFactor::APPROACHING, ""); output.path_candidate = shifted_path.path; return output; @@ -2270,17 +1018,15 @@ void AvoidanceModule::updatePathShifter(const AvoidLineArray & shift_lines) addNewShiftLines(path_shifter_, shift_lines); - current_raw_shift_lines_ = avoid_data_.raw_shift_line; - - registerRawShiftLines(shift_lines); + generator_.setRawRegisteredShiftLine(shift_lines, avoid_data_); - const auto sl = helper_.getMainShiftLine(shift_lines); + const auto sl = helper_->getMainShiftLine(shift_lines); const auto sl_front = shift_lines.front(); const auto sl_back = shift_lines.back(); - if (helper_.getRelativeShiftToPath(sl) > 0.0) { + if (helper_->getRelativeShiftToPath(sl) > 0.0) { left_shift_array_.push_back({uuid_map_.at("left"), sl_front.start, sl_back.end}); - } else if (helper_.getRelativeShiftToPath(sl) < 0.0) { + } else if (helper_->getRelativeShiftToPath(sl) < 0.0) { right_shift_array_.push_back({uuid_map_.at("right"), sl_front.start, sl_back.end}); } @@ -2348,101 +1094,15 @@ void AvoidanceModule::addNewShiftLines( const double road_velocity = avoid_data_.reference_path.points.at(front_new_shift_line.start_idx) .point.longitudinal_velocity_mps; const double shift_time = PathShifter::calcShiftTimeFromJerk( - front_new_shift_line.getRelativeLength(), helper_.getLateralMaxJerkLimit(), - helper_.getLateralMaxAccelLimit()); + front_new_shift_line.getRelativeLength(), helper_->getLateralMaxJerkLimit(), + helper_->getLateralMaxAccelLimit()); const double longitudinal_acc = std::clamp(road_velocity / shift_time, 0.0, parameters_->max_acceleration); path_shifter.setShiftLines(future); path_shifter.setVelocity(getEgoSpeed()); path_shifter.setLongitudinalAcceleration(longitudinal_acc); - path_shifter.setLateralAccelerationLimit(helper_.getLateralMaxAccelLimit()); -} - -AvoidLineArray AvoidanceModule::findNewShiftLine( - const AvoidLineArray & shift_lines, DebugData & debug) const -{ - if (shift_lines.empty()) { - return {}; - } - - // add small shift lines. - const auto add_straight_shift = - [&, this](auto & subsequent, bool has_large_shift, const size_t start_idx) { - for (size_t i = start_idx; i < shift_lines.size(); ++i) { - if ( - std::abs(shift_lines.at(i).getRelativeLength()) > - parameters_->lateral_small_shift_threshold) { - if (has_large_shift) { - return; - } - - has_large_shift = true; - } - - if (!isComfortable(AvoidLineArray{shift_lines.at(i)})) { - return; - } - - subsequent.push_back(shift_lines.at(i)); - } - }; - - // get subsequent shift lines. - const auto get_subsequent_shift = [&, this](size_t i) { - AvoidLineArray subsequent{shift_lines.at(i)}; - - if (!isComfortable(subsequent)) { - return subsequent; - } - - if (shift_lines.size() == i + 1) { - return subsequent; - } - - if (!isComfortable(AvoidLineArray{shift_lines.at(i + 1)})) { - return subsequent; - } - - if ( - std::abs(shift_lines.at(i).getRelativeLength()) < - parameters_->lateral_small_shift_threshold) { - const auto has_large_shift = - shift_lines.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; - - // candidate.at(i) is small length shift line. add large length shift line. - subsequent.push_back(shift_lines.at(i + 1)); - add_straight_shift(subsequent, has_large_shift, i + 2); - } else { - // candidate.at(i) is large length shift line. add small length shift lines. - add_straight_shift(subsequent, true, i + 1); - } - - return subsequent; - }; - - // check ignore or not. - const auto is_ignore_shift = [this](const auto & s) { - return std::abs(helper_.getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; - }; - - for (size_t i = 0; i < shift_lines.size(); ++i) { - const auto & candidate = shift_lines.at(i); - - // prevent sudden steering. - if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) { - break; - } - - if (!is_ignore_shift(candidate)) { - const auto new_shift_lines = get_subsequent_shift(i); - debug.step4_new_shift_line = new_shift_lines; - return new_shift_lines; - } - } - - DEBUG_PRINT("No new shift point exists."); - return {}; + path_shifter.setLateralAccelerationLimit(helper_->getLateralMaxAccelLimit()); } bool AvoidanceModule::isValidShiftLine( @@ -2467,7 +1127,7 @@ bool AvoidanceModule::isValidShiftLine( const auto new_shift_length = proposed_shift_path.shift_length.at(new_idx); constexpr double THRESHOLD = 0.1; - const auto offset = std::abs(new_shift_length - helper_.getEgoShift()); + const auto offset = std::abs(new_shift_length - helper_->getEgoShift()); if (offset > THRESHOLD) { RCLCPP_DEBUG_THROTTLE( getLogger(), *clock_, 1000, "new shift line is invalid. [HUGE OFFSET (%.2f)]", offset); @@ -2508,13 +1168,13 @@ void AvoidanceModule::updateData() { using utils::avoidance::toShiftedPath; - helper_.setData(planner_data_); + helper_->setData(planner_data_); - if (!helper_.isInitialized()) { - helper_.setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_.setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); - helper_.setPreviousReferencePath(*getPreviousModuleOutput().path); - helper_.setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( + if (!helper_->isInitialized()) { + helper_->setPreviousSplineShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); + helper_->setPreviousLinearShiftPath(toShiftedPath(*getPreviousModuleOutput().path)); + helper_->setPreviousReferencePath(*getPreviousModuleOutput().path); + helper_->setPreviousDrivingLanes(utils::avoidance::getCurrentLanesFromPath( *getPreviousModuleOutput().reference_path, planner_data_)); } @@ -2529,8 +1189,11 @@ void AvoidanceModule::updateData() return; } - // update registered shift point for new reference path & remove past objects - updateRegisteredRawShiftLines(); + // update shift line generator. + generator_.setData(planner_data_); + generator_.setPathShifter(path_shifter_); + generator_.setHelper(helper_); + generator_.update(avoid_data_, debug_data_); // update shift line and check path safety. fillShiftLine(avoid_data_, debug_data_); @@ -2561,16 +1224,13 @@ void AvoidanceModule::processOnExit() void AvoidanceModule::initVariables() { - helper_.reset(); + helper_->reset(); + generator_.reset(); path_shifter_ = PathShifter{}; - debug_data_ = DebugData(); debug_marker_.markers.clear(); resetPathCandidate(); resetPathReference(); - registered_raw_shift_lines_ = {}; - current_raw_shift_lines_ = {}; - original_unique_id = 0; is_avoidance_maneuver_starts = false; arrived_path_end_ = false; } @@ -2588,7 +1248,7 @@ void AvoidanceModule::updateRTCData() { const auto & data = avoid_data_; - updateRegisteredRTCStatus(helper_.getPreviousSplineShiftPath().path); + updateRegisteredRTCStatus(helper_->getPreviousSplineShiftPath().path); const auto candidates = data.safe ? data.safe_shift_line : data.new_shift_line; @@ -2597,10 +1257,10 @@ void AvoidanceModule::updateRTCData() return; } - const auto shift_line = helper_.getMainShiftLine(candidates); - if (helper_.getRelativeShiftToPath(shift_line) > 0.0) { + const auto shift_line = helper_->getMainShiftLine(candidates); + if (helper_->getRelativeShiftToPath(shift_line) > 0.0) { removePreviousRTCStatusRight(); - } else if (helper_.getRelativeShiftToPath(shift_line) < 0.0) { + } else if (helper_->getRelativeShiftToPath(shift_line) < 0.0) { removePreviousRTCStatusLeft(); } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); @@ -2612,7 +1272,7 @@ void AvoidanceModule::updateRTCData() const auto sl_back = candidates.back(); output.path_candidate = data.candidate_path.path; - output.lateral_shift = helper_.getRelativeShiftToPath(shift_line); + output.lateral_shift = helper_->getRelativeShiftToPath(shift_line); output.start_distance_to_path_change = sl_front.start_longitudinal; output.finish_distance_to_path_change = sl_back.end_longitudinal; @@ -2630,7 +1290,7 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con const size_t start_idx = front_shift_line.start_idx; const size_t end_idx = front_shift_line.end_idx; - const auto current_shift_length = helper_.getEgoShift(); + const auto current_shift_length = helper_->getEgoShift(); const double start_shift_length = path.shift_length.at(start_idx); const double end_shift_length = path.shift_length.at(end_idx); const double segment_shift_length = end_shift_length - start_shift_length; @@ -2743,137 +1403,13 @@ void AvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const void AvoidanceModule::updateDebugMarker( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { - using marker_utils::createLaneletsAreaMarkerArray; - using marker_utils::createObjectsMarkerArray; - using marker_utils::createPathMarkerArray; - using marker_utils::createPolygonMarkerArray; - using marker_utils::createPoseMarkerArray; - using marker_utils::createShiftGradMarkerArray; - using marker_utils::createShiftLengthMarkerArray; - using marker_utils::createShiftLineMarkerArray; - using marker_utils::showPolygon; - using marker_utils::showPredictedPath; - using marker_utils::showSafetyCheckInfo; - using marker_utils::avoidance_marker::createAvoidLineMarkerArray; - using marker_utils::avoidance_marker::createEgoStatusMarkerArray; - using marker_utils::avoidance_marker::createOtherObjectsMarkerArray; - using marker_utils::avoidance_marker::createOverhangFurthestLineStringMarkerArray; - using marker_utils::avoidance_marker::createPredictedVehiclePositions; - using marker_utils::avoidance_marker::makeOverhangToRoadShoulderMarkerArray; - using tier4_autoware_utils::appendMarkerArray; - debug_marker_.markers.clear(); if (!parameters_->publish_debug_marker) { return; } - const auto & path = data.reference_path; - - const auto current_time = rclcpp::Clock{RCL_ROS_TIME}.now(); - - const auto add = [this](const MarkerArray & added) { appendMarkerArray(added, &debug_marker_); }; - - const auto addAvoidLine = - [&](const AvoidLineArray & al_arr, const auto & ns, auto r, auto g, auto b, double w = 0.05) { - add(createAvoidLineMarkerArray(al_arr, ns, r, g, b, w)); - }; - - const auto addShiftLine = - [&](const ShiftLineArray & sl_arr, const auto & ns, auto r, auto g, auto b, double w = 0.1) { - add(createShiftLineMarkerArray(sl_arr, shifter.getBaseOffset(), ns, r, g, b, w)); - }; - - const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) { - add(createOtherObjectsMarkerArray(objects, ns)); - }; - - const auto addShiftLength = - [&](const auto & shift_length, const auto & ns, auto r, auto g, auto b) { - add(createShiftLengthMarkerArray(shift_length, path, ns, r, g, b)); - }; - - const auto addShiftGrad = [&]( - const auto & shift_grad, const auto & shift_length, const auto & ns, - auto r, auto g, auto b) { - add(createShiftGradMarkerArray(shift_grad, shift_length, path, ns, r, g, b)); - }; - - // ignore objects - { - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE); - addObjects(data.other_objects, AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL); - addObjects(data.other_objects, AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE); - addObjects(data.other_objects, AvoidanceDebugFactor::NOT_PARKING_OBJECT); - addObjects(data.other_objects, std::string("MovingObject")); - addObjects(data.other_objects, std::string("CrosswalkUser")); - addObjects(data.other_objects, std::string("OutOfTargetArea")); - addObjects(data.other_objects, std::string("NotNeedAvoidance")); - addObjects(data.other_objects, std::string("LessThanExecutionThreshold")); - addObjects(data.other_objects, std::string("TooNearToGoal")); - } - - // shift line pre-process - { - addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0); - addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3); - addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3); - addAvoidLine(debug.step1_filled_shift_line, "step1_filled_shift_line", 0.2, 0.8, 0.4, 0.3); - addAvoidLine(debug.step1_return_shift_line, "step1_return_shift_line", 0.2, 1.0, 0.2, 0.3); - } - - // merge process - { - addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3); - } - - // trimming process - { - addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3); - addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3); - addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3); - } - - // registering process - { - addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3); - addAvoidLine(data.raw_shift_line, "step4_raw_shift_line", 1.0, 0.0, 0.0, 0.3); - addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3); - } - - // safety check - { - add(showSafetyCheckInfo(debug.collision_check, "object_debug_info")); - add(showPredictedPath(debug.collision_check, "ego_predicted_path")); - add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation")); - } - - // shift length - { - addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5); - addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7); - addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2); - } - - // shift grad - { - addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5); - addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7); - addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2); - addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9); - } - - // misc - { - add(createEgoStatusMarkerArray(data, getEgoPose(), "ego_status")); - add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5)); - add(createLaneletsAreaMarkerArray(*debug.current_lanelets, "current_lanelet", 0.0, 1.0, 0.0)); - add(createPolygonMarkerArray(debug.detection_area, "detection_area", 0L, 0.16, 1.0, 0.69, 0.1)); - add(makeOverhangToRoadShoulderMarkerArray(data.target_objects, "overhang")); - add(createOverhangFurthestLineStringMarkerArray(debug.bounds, "bounds", 1.0, 0.0, 1.0)); - } + debug_marker_ = marker_utils::avoidance_marker::createDebugMarkerArray(data, shifter, debug); } void AvoidanceModule::updateAvoidanceDebugData( @@ -2919,8 +1455,8 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto variable = helper_.getMinAvoidanceDistance( - helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); + const auto variable = helper_->getMinAvoidanceDistance( + helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin)); const auto & additional_buffer_longitudinal = object_parameter.use_conservative_buffer_longitudinal ? planner_data_->parameters.base_link2front @@ -2955,7 +1491,7 @@ void AvoidanceModule::insertReturnDeadLine( shifted_path.path.points, getEgoPosition(), shifted_path.path.points.size() - 1); const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end); - const auto min_return_distance = helper_.getMinAvoidanceDistance(shift_length); + const auto min_return_distance = helper_->getMinAvoidanceDistance(shift_length); const auto to_stop_line = data.to_return_point - min_return_distance - buffer; // If we don't need to consider deceleration constraints, insert a deceleration point @@ -2968,7 +1504,7 @@ void AvoidanceModule::insertReturnDeadLine( } // If the stop distance is not enough for comfortable stop, don't insert wait point. - const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < to_stop_line; + const auto is_comfortable_stop = helper_->getFeasibleDecelDistance(0.0) < to_stop_line; if (!is_comfortable_stop) { RCLCPP_DEBUG(getLogger(), "stop distance is not enough."); return; @@ -2979,7 +1515,7 @@ void AvoidanceModule::insertReturnDeadLine( // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, helper_.getLateralMinJerkLimit(), to_stop_line); + shift_length, helper_->getLateralMinJerkLimit(), to_stop_line); if (current_target_velocity < getEgoSpeed()) { RCLCPP_DEBUG(getLogger(), "current velocity exceeds target slow down speed."); return; @@ -2997,7 +1533,7 @@ void AvoidanceModule::insertReturnDeadLine( // target speed with nominal jerk limits. const double v_target = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, parameters_->min_slow_down_speed); @@ -3015,7 +1551,7 @@ void AvoidanceModule::insertWaitPoint( return; } - if (helper_.isShifted()) { + if (helper_->isShifted()) { return; } @@ -3028,7 +1564,7 @@ void AvoidanceModule::insertWaitPoint( } // If the stop distance is not enough for comfortable stop, don't insert wait point. - const auto is_comfortable_stop = helper_.getFeasibleDecelDistance(0.0) < data.to_stop_line; + const auto is_comfortable_stop = helper_->getFeasibleDecelDistance(0.0) < data.to_stop_line; const auto is_slow_speed = getEgoSpeed() < parameters_->min_slow_down_speed; if (!is_comfortable_stop && !is_slow_speed) { RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "not execute uncomfortable deceleration."); @@ -3036,7 +1572,7 @@ void AvoidanceModule::insertWaitPoint( } // If target object can be stopped for, insert a deceleration point and return - if (data.stop_target_object.get().is_stoppable) { + if (data.stop_target_object.value().is_stoppable) { utils::avoidance::insertDecelPoint( getEgoPosition(), data.to_stop_line, 0.0, shifted_path.path, stop_pose_); return; @@ -3044,7 +1580,7 @@ void AvoidanceModule::insertWaitPoint( // If the object cannot be stopped for, calculate a "mild" deceleration distance // and insert a deceleration point at that distance - const auto stop_distance = helper_.getFeasibleDecelDistance(0.0, false); + const auto stop_distance = helper_->getFeasibleDecelDistance(0.0, false); utils::avoidance::insertDecelPoint( getEgoPosition(), stop_distance, 0.0, shifted_path.path, stop_pose_); } @@ -3087,7 +1623,7 @@ void AvoidanceModule::insertStopPoint( } // Otherwise, consider deceleration constraints before inserting deceleration point - const auto decel_distance = helper_.getFeasibleDecelDistance(0.0, false); + const auto decel_distance = helper_->getFeasibleDecelDistance(0.0, false); if (stop_distance < decel_distance) { return; } @@ -3106,11 +1642,11 @@ void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const return; } - if (helper_.isShifted()) { + if (helper_->isShifted()) { return; } - const auto decel_distance = helper_.getFeasibleDecelDistance(p->yield_velocity, false); + const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false); utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_); } @@ -3125,7 +1661,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const } // insert slow down speed only when the avoidance maneuver is not initiated. - if (helper_.isShifted()) { + if (helper_->isShifted()) { return; } @@ -3149,13 +1685,13 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; const auto shift_length = - helper_.getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); + helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin); // check slow down feasibility - const auto min_avoid_distance = helper_.getMinAvoidanceDistance(shift_length); + const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length); const auto distance_to_object = object.longitudinal; const auto remaining_distance = distance_to_object - min_avoid_distance; - const auto decel_distance = helper_.getFeasibleDecelDistance(parameters_->velocity_map.front()); + const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front()); if (remaining_distance < decel_distance) { return; } @@ -3165,7 +1701,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // insert slow down speed. const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, helper_.getLateralMinJerkLimit(), distance_to_object); + shift_length, helper_->getLateralMinJerkLimit(), distance_to_object); if (current_target_velocity < getEgoSpeed() && decel_distance < remaining_distance) { utils::avoidance::insertDecelPoint( getEgoPosition(), decel_distance, parameters_->velocity_map.front(), shifted_path.path, @@ -3185,7 +1721,7 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const // target speed with nominal jerk limits. const double v_target = PathShifter::calcFeasibleVelocityFromJerk( - shift_length, helper_.getLateralMinJerkLimit(), shift_longitudinal_distance); + shift_length, helper_->getLateralMinJerkLimit(), shift_longitudinal_distance); const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps; const double v_insert = std::max(v_target - parameters_->buf_slow_down_speed, lower_speed); diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index f71024dcc5034..e17804f10deaa 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -25,18 +25,18 @@ namespace behavior_path_planner { - -AvoidanceModuleManager::AvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) -: SceneModuleManagerInterface(node, name, config, {"left", "right"}) +void AvoidanceModuleManager::init(rclcpp::Node * node) { using autoware_auto_perception_msgs::msg::ObjectClassification; using tier4_autoware_utils::getOrDeclareParameter; + // init manager interface + initInterface(node, {"left", "right"}); + AvoidanceParameters p{}; // general params { - std::string ns = "avoidance."; + const std::string ns = "avoidance."; p.resample_interval_for_planning = getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = @@ -53,7 +53,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // drivable area { - std::string ns = "avoidance."; + const std::string ns = "avoidance."; p.use_adjacent_lane = getOrDeclareParameter(*node, ns + "use_adjacent_lane"); p.use_opposite_lane = getOrDeclareParameter(*node, ns + "use_opposite_lane"); p.use_intersection_areas = getOrDeclareParameter(*node, ns + "use_intersection_areas"); @@ -65,7 +65,6 @@ AvoidanceModuleManager::AvoidanceModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = getOrDeclareParameter(*node, ns + "is_target"); param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "moving_speed_threshold"); @@ -105,7 +104,24 @@ AvoidanceModuleManager::AvoidanceModuleManager( // target filtering { - std::string ns = "avoidance.target_filtering."; + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -114,12 +130,14 @@ AvoidanceModuleManager::AvoidanceModuleManager( getOrDeclareParameter(*node, ns + "object_check_shiftable_ratio"); p.object_check_min_road_shoulder_width = getOrDeclareParameter(*node, ns + "object_check_min_road_shoulder_width"); + p.object_check_yaw_deviation = + getOrDeclareParameter(*node, ns + "intersection.yaw_deviation"); p.object_last_seen_threshold = getOrDeclareParameter(*node, ns + "object_last_seen_threshold"); } { - std::string ns = "avoidance.target_filtering.force_avoidance."; + const std::string ns = "avoidance.target_filtering.force_avoidance."; p.enable_force_avoidance_for_stopped_vehicle = getOrDeclareParameter(*node, ns + "enable"); p.threshold_time_force_avoidance_for_stopped_vehicle = @@ -133,7 +151,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( } { - std::string ns = "avoidance.target_filtering.detection_area."; + const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); p.object_check_min_forward_distance = getOrDeclareParameter(*node, ns + "min_forward_distance"); @@ -145,7 +163,24 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check general params { - std::string ns = "avoidance.safety_check."; + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_safety_check_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.safety_check."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.enable_safety_check = getOrDeclareParameter(*node, ns + "enable"); p.check_current_lane = getOrDeclareParameter(*node, ns + "check_current_lane"); p.check_shift_side_lane = getOrDeclareParameter(*node, ns + "check_shift_side_lane"); @@ -165,7 +200,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check predicted path params { - std::string ns = "avoidance.safety_check."; + const std::string ns = "avoidance.safety_check."; p.ego_predicted_path_params.min_velocity = getOrDeclareParameter(*node, ns + "min_velocity"); p.ego_predicted_path_params.max_velocity = @@ -182,7 +217,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // safety check rss params { - std::string ns = "avoidance.safety_check."; + const std::string ns = "avoidance.safety_check."; p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter(*node, ns + "longitudinal_distance_min_threshold"); p.rss_params.longitudinal_velocity_delta_time = @@ -201,7 +236,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (lateral) { - std::string ns = "avoidance.avoidance.lateral."; + const std::string ns = "avoidance.avoidance.lateral."; p.soft_road_shoulder_margin = getOrDeclareParameter(*node, ns + "soft_road_shoulder_margin"); p.hard_road_shoulder_margin = @@ -220,7 +255,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (longitudinal) { - std::string ns = "avoidance.avoidance.longitudinal."; + const std::string ns = "avoidance.avoidance.longitudinal."; p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); @@ -232,7 +267,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (return shift dead line) { - std::string ns = "avoidance.avoidance.return_dead_line."; + const std::string ns = "avoidance.avoidance.return_dead_line."; p.enable_dead_line_for_goal = getOrDeclareParameter(*node, ns + "goal.enable"); p.enable_dead_line_for_traffic_light = getOrDeclareParameter(*node, ns + "traffic_light.enable"); @@ -243,20 +278,21 @@ AvoidanceModuleManager::AvoidanceModuleManager( // yield { - std::string ns = "avoidance.yield."; + const std::string ns = "avoidance.yield."; p.yield_velocity = getOrDeclareParameter(*node, ns + "yield_velocity"); } // stop { - std::string ns = "avoidance.stop."; + const std::string ns = "avoidance.stop."; p.stop_max_distance = getOrDeclareParameter(*node, ns + "max_distance"); p.stop_buffer = getOrDeclareParameter(*node, ns + "stop_buffer"); } // policy { - std::string ns = "avoidance.policy."; + const std::string ns = "avoidance.policy."; + p.policy_approval = getOrDeclareParameter(*node, ns + "make_approval_request"); p.policy_deceleration = getOrDeclareParameter(*node, ns + "deceleration"); p.policy_lateral_margin = getOrDeclareParameter(*node, ns + "lateral_margin"); p.use_shorten_margin_immediately = @@ -273,7 +309,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // constraints (longitudinal) { - std::string ns = "avoidance.constraints.longitudinal."; + const std::string ns = "avoidance.constraints.longitudinal."; p.nominal_deceleration = getOrDeclareParameter(*node, ns + "nominal_deceleration"); p.nominal_jerk = getOrDeclareParameter(*node, ns + "nominal_jerk"); p.max_deceleration = getOrDeclareParameter(*node, ns + "max_deceleration"); @@ -283,7 +319,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // constraints (lateral) { - std::string ns = "avoidance.constraints.lateral."; + const std::string ns = "avoidance.constraints.lateral."; p.velocity_map = getOrDeclareParameter>(*node, ns + "velocity"); p.lateral_max_accel_map = getOrDeclareParameter>(*node, ns + "max_accel_values"); @@ -311,7 +347,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( // shift line pipeline { - std::string ns = "avoidance.shift_line_pipeline."; + const std::string ns = "avoidance.shift_line_pipeline."; p.quantize_filter_threshold = getOrDeclareParameter(*node, ns + "trim.quantize_filter_threshold"); p.same_grad_filter_1_threshold = @@ -344,7 +380,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorobject_parameters.at(semantic); - updateParam(parameters, ns + "is_target", config.is_target); updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); @@ -451,3 +486,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceModuleManager, behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp index f917f1e2f3c65..de76166e068d3 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp @@ -14,8 +14,8 @@ #include "behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include "signal_processing/lowpass_filter_1d.hpp" @@ -222,13 +222,36 @@ std::optional getObstacleFromUuid( } return *itr; } + +double calcDistanceToSegment( + const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2_first, + const geometry_msgs::msg::Point & p2_second) +{ + const Eigen::Vector2d first_to_target(p1.x - p2_first.x, p1.y - p2_first.y); + const Eigen::Vector2d second_to_target(p1.x - p2_second.x, p1.y - p2_second.y); + const Eigen::Vector2d first_to_second(p2_second.x - p2_first.x, p2_second.y - p2_first.y); + + if (first_to_target.dot(first_to_second) < 0) { + return first_to_target.norm(); + } + if (second_to_target.dot(-first_to_second) < 0) { + return second_to_target.norm(); + } + + const Eigen::Vector2d p2_nearest = + Eigen::Vector2d{p2_first.x, p2_first.y} + + first_to_second * first_to_target.dot(first_to_second) / std::pow(first_to_second.norm(), 2); + return (Eigen::Vector2d{p1.x, p1.y} - p2_nearest).norm(); +} } // namespace DynamicAvoidanceModule::DynamicAvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, target_objects_manager_{TargetObjectsManager( parameters_->successive_num_to_entry_dynamic_avoidance_condition, @@ -240,13 +263,13 @@ bool DynamicAvoidanceModule::isExecutionRequested() const { RCLCPP_DEBUG(getLogger(), "DYNAMIC AVOIDANCE isExecutionRequested."); - const auto prev_module_path = getPreviousModuleOutput().path; - if (!prev_module_path || prev_module_path->points.size() < 2) { + const auto input_path = getPreviousModuleOutput().path; + if (!input_path || input_path->points.size() < 2) { return false; } // check if the ego is driving forward - const auto is_driving_forward = motion_utils::isDrivingForward(prev_module_path->points); + const auto is_driving_forward = motion_utils::isDrivingForward(input_path->points); if (!is_driving_forward || !(*is_driving_forward)) { return false; } @@ -290,16 +313,16 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() info_marker_.markers.clear(); debug_marker_.markers.clear(); - const auto prev_module_path = getPreviousModuleOutput().path; + const auto & input_path = getPreviousModuleOutput().path; // create obstacles to avoid (= extract from the drivable area) std::vector obstacles_for_drivable_area; for (const auto & object : target_objects_) { const auto obstacle_poly = [&]() { - if (object.polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { + if (parameters_->polygon_generation_method == PolygonGenerationMethod::EGO_PATH_BASE) { return calcEgoPathBasedDynamicObstaclePolygon(object); } - if (object.polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { + if (parameters_->polygon_generation_method == PolygonGenerationMethod::OBJECT_PATH_BASE) { return calcObjectPathBasedDynamicObstaclePolygon(object); } throw std::logic_error("The polygon_generation_method's string is invalid."); @@ -321,7 +344,7 @@ BehaviorModuleOutput DynamicAvoidanceModule::plan() parameters_->use_hatched_road_markings; BehaviorModuleOutput output; - output.path = prev_module_path; + output.path = input_path; output.drivable_area_info = utils::combineDrivableAreaInfo( current_drivable_area_info, getPreviousModuleOutput().drivable_area_info); output.reference_path = getPreviousModuleOutput().reference_path; @@ -375,10 +398,10 @@ bool DynamicAvoidanceModule::isLabelTargetObstacle(const uint8_t label) const void DynamicAvoidanceModule::updateTargetObjects() { - const auto prev_module_path = getPreviousModuleOutput().path; + const auto input_path = getPreviousModuleOutput().path; const auto & predicted_objects = planner_data_->dynamic_object->objects; - const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + const auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; const auto prev_objects = target_objects_manager_.getValidObjects(); // 1. Rough filtering of target objects @@ -404,7 +427,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 1.b. check obstacle velocity const auto [obj_tangent_vel, obj_normal_vel] = - projectObstacleVelocityToTrajectory(prev_module_path->points, predicted_object); + projectObstacleVelocityToTrajectory(input_path->points, predicted_object); if ( std::abs(obj_tangent_vel) < parameters_->min_obstacle_vel || parameters_->max_obstacle_vel < std::abs(obj_tangent_vel)) { @@ -412,7 +435,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.c. check if object is not crossing ego's path - const double obj_angle = calcDiffAngleBetweenPaths(prev_module_path->points, obj_path); + const double obj_angle = calcDiffAngleBetweenPaths(input_path->points, obj_path); const double max_crossing_object_angle = 0.0 <= obj_tangent_vel ? parameters_->max_overtaking_crossing_object_angle : parameters_->max_oncoming_crossing_object_angle; @@ -432,7 +455,7 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 1.e. check if object lateral offset to ego's path is small enough - const double obj_dist_to_path = calcDistanceToPath(prev_module_path->points, obj_pose.position); + const double obj_dist_to_path = calcDistanceToPath(input_path->points, obj_pose.position); const bool is_object_far_from_path = isObjectFarFromPath(predicted_object, obj_dist_to_path); if (is_object_far_from_path) { RCLCPP_INFO_EXPRESSION( @@ -469,7 +492,6 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2. Precise filtering of target objects and check if they should be avoided for (const auto & object : target_objects_manager_.getValidObjects()) { - PolygonGenerationMethod polygon_generation_method{PolygonGenerationMethod::EGO_PATH_BASE}; const auto obj_uuid = object.uuid; const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid); const auto obj_path = *std::max_element( @@ -477,11 +499,13 @@ void DynamicAvoidanceModule::updateTargetObjects() [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); // 2.a. check if object is not to be followed by ego - const double obj_angle = calcDiffAngleAgainstPath(prev_module_path->points, object.pose); + const double obj_angle = calcDiffAngleAgainstPath(input_path->points, object.pose); const bool is_object_aligned_to_path = std::abs(obj_angle) < parameters_->max_front_object_angle || M_PI - parameters_->max_front_object_angle < std::abs(obj_angle); - if (object.is_object_on_ego_path && is_object_aligned_to_path) { + if ( + object.is_object_on_ego_path && is_object_aligned_to_path && + parameters_->min_front_object_vel < object.vel) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, "[DynamicAvoidance] Ignore obstacle (%s) since it is to be followed.", obj_uuid.c_str()); @@ -489,13 +513,13 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.b. calculate which side object exists against ego's path - const bool is_object_left = isLeft(prev_module_path->points, object.pose.position); + const bool is_object_left = isLeft(input_path->points, object.pose.position); const auto lat_lon_offset = - getLateralLongitudinalOffset(prev_module_path->points, object.pose, object.shape); + getLateralLongitudinalOffset(input_path->points, object.pose, object.shape); // 2.c. check if object will not cut in - const bool will_object_cut_in = willObjectCutIn( - prev_module_path->points, obj_path, object.vel, lat_lon_offset, polygon_generation_method); + const bool will_object_cut_in = + willObjectCutIn(input_path->points, obj_path, object.vel, lat_lon_offset); if (will_object_cut_in) { RCLCPP_INFO_EXPRESSION( getLogger(), parameters_->enable_debug_info, @@ -513,7 +537,7 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.e. check time to collision const double time_to_collision = - calcTimeToCollision(prev_module_path->points, object.pose, object.vel, lat_lon_offset); + calcTimeToCollision(input_path->points, object.pose, object.vel, lat_lon_offset); if ( (0 <= object.vel && parameters_->max_time_to_collision_overtaking_object < time_to_collision) || @@ -536,15 +560,14 @@ void DynamicAvoidanceModule::updateTargetObjects() // 2.f. calculate which side object will be against ego's path const auto future_obj_pose = object_recognition_utils::calcInterpolatedPose(obj_path, time_to_collision); - const bool is_collision_left = future_obj_pose - ? isLeft(prev_module_path->points, future_obj_pose->position) - : is_object_left; + const bool is_collision_left = + future_obj_pose ? isLeft(input_path->points, future_obj_pose->position) : is_object_left; // 2.g. check if the ego is not ahead of the object. const double signed_dist_ego_to_obj = [&]() { - const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(prev_module_path->points); + const size_t ego_seg_idx = planner_data_->findEgoSegmentIndex(input_path->points); const double lon_offset_ego_to_obj = motion_utils::calcSignedArcLength( - prev_module_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); + input_path->points, getEgoPose().position, ego_seg_idx, lat_lon_offset.nearest_idx); if (0 < lon_offset_ego_to_obj) { return std::max( 0.0, lon_offset_ego_to_obj - planner_data_->parameters.front_overhang + @@ -564,18 +587,29 @@ void DynamicAvoidanceModule::updateTargetObjects() } // 2.h. calculate longitudinal and lateral offset to avoid to generate object polygon by - // "object_path_base" + // "ego_path_base" const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid( - path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision); + input_ref_path_points, object.pose, obj_points, object.vel, obj_path, object.shape, + time_to_collision); const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid( - path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object); + input_ref_path_points, obj_points, object.vel, is_collision_left, object.lat_vel, + prev_object); + if (!lat_offset_to_avoid) { + RCLCPP_INFO_EXPRESSION( + getLogger(), parameters_->enable_debug_info, + "[DynamicAvoidance] Ignore obstacle (%s) since the object laterally covers the ego's path " + "enough", + obj_uuid.c_str()); + continue; + } const bool should_be_avoided = true; target_objects_manager_.updateObject( - obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided, - polygon_generation_method); + obj_uuid, lon_offset_to_avoid, *lat_offset_to_avoid, is_collision_left, should_be_avoided); } + + prev_input_ref_path_points = input_ref_path_points; } [[maybe_unused]] std::optional> @@ -650,9 +684,13 @@ bool DynamicAvoidanceModule::isObjectFarFromPath( bool DynamicAvoidanceModule::willObjectCutIn( const std::vector & ego_path, const PredictedPath & predicted_path, - const double obj_tangent_vel, const LatLonOffset & lat_lon_offset, - PolygonGenerationMethod & polygon_generation_method) const + const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const { + // Ignore oncoming object + if (obj_tangent_vel < parameters_->min_cut_in_object_vel) { + return false; + } + // Check if ego's path and object's path are close. const bool will_object_cut_in = [&]() { for (const auto & predicted_path_point : predicted_path.path) { @@ -680,7 +718,6 @@ bool DynamicAvoidanceModule::willObjectCutIn( lon_offset_ego_to_obj < std::max( parameters_->min_lon_offset_ego_to_cut_in_object, relative_velocity * parameters_->min_time_to_start_cut_in)) { - polygon_generation_method = PolygonGenerationMethod::EGO_PATH_BASE; return false; } @@ -692,7 +729,7 @@ DynamicAvoidanceModule::DecisionWithReason DynamicAvoidanceModule::willObjectCut const std::optional & prev_object) const { // Ignore oncoming object - if (obj_tangent_vel < 0) { + if (obj_tangent_vel < parameters_->min_cut_out_object_vel) { return DecisionWithReason{false}; } @@ -750,13 +787,13 @@ std::pair DynamicAvoidanceModule // left lane const auto opt_left_lane = rh->getLeftLanelet(lane); if (opt_left_lane) { - left_lanes.push_back(opt_left_lane.get()); + left_lanes.push_back(opt_left_lane.value()); } // right lane const auto opt_right_lane = rh->getRightLanelet(lane); if (opt_right_lane) { - right_lanes.push_back(opt_right_lane.get()); + right_lanes.push_back(opt_right_lane.value()); } const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); @@ -803,12 +840,13 @@ DynamicAvoidanceModule::LatLonOffset DynamicAvoidanceModule::getLateralLongitudi } MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( - const std::vector & path_points_for_object_polygon, + const std::vector & input_ref_path_points, const geometry_msgs::msg::Pose & obj_pose, const Polygon2d & obj_points, const double obj_vel, + const PredictedPath & obj_path, const autoware_auto_perception_msgs::msg::Shape & obj_shape, const double time_to_collision) const { const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, obj_pose.position); + motion_utils::findNearestSegmentIndex(input_ref_path_points, obj_pose.position); // calculate min/max longitudinal offset from object to path const auto obj_lon_offset = [&]() { @@ -816,7 +854,7 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const double lon_offset = motion_utils::calcLongitudinalOffsetToSegment( - path_points_for_object_polygon, obj_seg_idx, geom_obj_point); + input_ref_path_points, obj_seg_idx, geom_obj_point); obj_lon_offset_vec.push_back(lon_offset); } @@ -844,47 +882,124 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLongitudinalOffsetToAvoid( std::abs(obj_vel) * (is_object_overtaking ? parameters_->end_duration_to_avoid_overtaking_object : parameters_->end_duration_to_avoid_oncoming_object); + if (obj_vel < 0) { + const double valid_start_length_to_avoid = + calcValidStartLengthToAvoid(obj_path, obj_pose, obj_shape); + return MinMaxValue{ + std::max(obj_lon_offset.min_value - start_length_to_avoid, valid_start_length_to_avoid), + obj_lon_offset.max_value + end_length_to_avoid}; + } return MinMaxValue{ obj_lon_offset.min_value - start_length_to_avoid, obj_lon_offset.max_value + end_length_to_avoid}; } -MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( - const std::vector & path_points_for_object_polygon, - const Polygon2d & obj_points, const bool is_collision_left, const double obj_normal_vel, +double DynamicAvoidanceModule::calcValidStartLengthToAvoid( + const PredictedPath & obj_path, const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape) const +{ + const auto input_path_points = getPreviousModuleOutput().path->points; + const size_t obj_seg_idx = + motion_utils::findNearestSegmentIndex(input_path_points, obj_pose.position); + + const size_t valid_obj_path_end_idx = [&]() { + int ego_path_idx = obj_seg_idx + 1; + for (size_t obj_path_idx = 0; obj_path_idx < obj_path.path.size(); ++obj_path_idx) { + bool are_paths_close{false}; + for (; 0 < ego_path_idx; --ego_path_idx) { + const double dist_to_segment = calcDistanceToSegment( + obj_path.path.at(obj_path_idx).position, + input_path_points.at(ego_path_idx).point.pose.position, + input_path_points.at(ego_path_idx - 1).point.pose.position); + if ( + dist_to_segment < planner_data_->parameters.vehicle_width / 2.0 + + parameters_->lat_offset_from_obstacle + + calcObstacleMaxLength(obj_shape)) { + are_paths_close = true; + break; + } + } + + if (!are_paths_close) { + return obj_path_idx; + } + } + return obj_path.path.size() - 1; + }(); + return -motion_utils::calcSignedArcLength(obj_path.path, 0, valid_obj_path_end_idx); +} + +std::optional DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( + const std::vector & input_ref_path_points, const Polygon2d & obj_points, + const double obj_vel, const bool is_collision_left, const double obj_normal_vel, const std::optional & prev_object) const { + const bool enable_lowpass_filter = [&]() { + if (prev_input_ref_path_points.size() < 2) { + return true; + } + const size_t prev_front_seg_idx = motion_utils::findNearestSegmentIndex( + prev_input_ref_path_points, input_ref_path_points.front().point.pose.position); + constexpr double min_lane_change_path_lat_offset = 1.0; + if ( + motion_utils::calcLateralOffset( + prev_input_ref_path_points, input_ref_path_points.front().point.pose.position, + prev_front_seg_idx) < min_lane_change_path_lat_offset) { + return true; + } + // NOTE: When the input reference path laterally changes, the low-pass filter is disabled not to + // shift the obstacle polygon suddenly. + return false; + }(); + // calculate min/max lateral offset from object to path const auto obj_lat_abs_offset = [&]() { std::vector obj_lat_abs_offset_vec; for (size_t i = 0; i < obj_points.outer().size(); ++i) { const auto geom_obj_point = toGeometryPoint(obj_points.outer().at(i)); const size_t obj_point_seg_idx = - motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, geom_obj_point); - const double obj_point_lat_offset = motion_utils::calcLateralOffset( - path_points_for_object_polygon, geom_obj_point, obj_point_seg_idx); - obj_lat_abs_offset_vec.push_back(std::abs(obj_point_lat_offset)); + motion_utils::findNearestSegmentIndex(input_ref_path_points, geom_obj_point); + const double obj_point_lat_offset = + motion_utils::calcLateralOffset(input_ref_path_points, geom_obj_point, obj_point_seg_idx); + obj_lat_abs_offset_vec.push_back(obj_point_lat_offset); } return getMinMaxValues(obj_lat_abs_offset_vec); }(); const double min_obj_lat_abs_offset = obj_lat_abs_offset.min_value; const double max_obj_lat_abs_offset = obj_lat_abs_offset.max_value; + if (parameters_->min_front_object_vel < obj_vel) { + const double obj_width_on_ego_path = + std::min(max_obj_lat_abs_offset, planner_data_->parameters.vehicle_width / 2.0) - + std::max(min_obj_lat_abs_offset, -planner_data_->parameters.vehicle_width / 2.0); + if ( + planner_data_->parameters.vehicle_width * + parameters_->max_front_object_ego_path_lat_cover_ratio < + obj_width_on_ego_path) { + return std::nullopt; + } + } + // calculate bound min and max lateral offset const double min_bound_lat_offset = [&]() { const double lat_abs_offset_to_shift = std::max(0.0, obj_normal_vel * (is_collision_left ? -1.0 : 1.0)) * parameters_->max_time_for_lat_shift; const double raw_min_bound_lat_offset = - min_obj_lat_abs_offset - parameters_->lat_offset_from_obstacle - lat_abs_offset_to_shift; + (is_collision_left ? min_obj_lat_abs_offset : max_obj_lat_abs_offset) - + (parameters_->lat_offset_from_obstacle + lat_abs_offset_to_shift) * + (is_collision_left ? 1.0 : -1.0); const double min_bound_lat_abs_offset_limit = planner_data_->parameters.vehicle_width / 2.0 - parameters_->max_lat_offset_to_avoid; - return std::max(raw_min_bound_lat_offset, min_bound_lat_abs_offset_limit) * - (is_collision_left ? 1.0 : -1.0); + + if (is_collision_left) { + return std::max(raw_min_bound_lat_offset, min_bound_lat_abs_offset_limit); + } + return std::min(raw_min_bound_lat_offset, -min_bound_lat_abs_offset_limit); }(); const double max_bound_lat_offset = - (max_obj_lat_abs_offset + parameters_->lat_offset_from_obstacle) * - (is_collision_left ? 1.0 : -1.0); + (is_collision_left ? max_obj_lat_abs_offset : min_obj_lat_abs_offset) + + (is_collision_left ? 1.0 : -1.0) * parameters_->lat_offset_from_obstacle; // filter min_bound_lat_offset const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional { @@ -894,10 +1009,11 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid( return prev_object->lat_offset_to_avoid->min_value; }(); const double filtered_min_bound_lat_offset = - prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter( - min_bound_lat_offset, *prev_min_lat_avoid_to_offset, - parameters_->lpf_gain_for_lat_avoid_to_offset) - : min_bound_lat_offset; + (prev_min_lat_avoid_to_offset.has_value() & enable_lowpass_filter) + ? signal_processing::lowpassFilter( + min_bound_lat_offset, *prev_min_lat_avoid_to_offset, + parameters_->lpf_gain_for_lat_avoid_to_offset) + : min_bound_lat_offset; return MinMaxValue{filtered_min_bound_lat_offset, max_bound_lat_offset}; } @@ -911,19 +1027,19 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( return std::nullopt; } - auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points; + auto input_ref_path_points = getPreviousModuleOutput().reference_path->points; const size_t obj_seg_idx = - motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position); + motion_utils::findNearestSegmentIndex(input_ref_path_points, object.pose.position); const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape); const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint( - obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon); + obj_seg_idx, object.lon_offset_to_avoid->min_value, input_ref_path_points); const size_t updated_obj_seg_idx = (lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1 : obj_seg_idx; const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint( - updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon); + updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, input_ref_path_points); if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) { // NOTE: The obstacle is longitudinally out of the ego's trajectory. @@ -931,22 +1047,22 @@ DynamicAvoidanceModule::calcEgoPathBasedDynamicObstaclePolygon( } const size_t lon_bound_start_idx = lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast(0); - const size_t lon_bound_end_idx = - lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value() - : static_cast(path_points_for_object_polygon.size() - 1); + const size_t lon_bound_end_idx = lon_bound_end_idx_opt + ? lon_bound_end_idx_opt.value() + : static_cast(input_ref_path_points.size() - 1); // create inner/outer bound points std::vector obj_inner_bound_points; std::vector obj_outer_bound_points; for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) { - obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->min_value, 0.0) - .position); - obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose( - path_points_for_object_polygon.at(i).point.pose, 0.0, - object.lat_offset_to_avoid->max_value, 0.0) - .position); + obj_inner_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->min_value, 0.0) + .position); + obj_outer_bound_points.push_back( + tier4_autoware_utils::calcOffsetPose( + input_ref_path_points.at(i).point.pose, 0.0, object.lat_offset_to_avoid->max_value, 0.0) + .position); } // create obj_polygon from inner/outer bound points diff --git a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp index 6f624a9f6f017..2824a6221591a 100644 --- a/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/dynamic_avoidance/manager.cpp @@ -24,21 +24,34 @@ namespace behavior_path_planner { +namespace +{ +PolygonGenerationMethod convertToPolygonGenerationMethod(const std::string & str) +{ + if (str == "ego_path_base") { + return PolygonGenerationMethod::EGO_PATH_BASE; + } else if (str == "object_path_base") { + return PolygonGenerationMethod::OBJECT_PATH_BASE; + } + throw std::logic_error("The polygon_generation_method's string is invalid."); +} +} // namespace -DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) -: SceneModuleManagerInterface(node, name, config, {""}) +void DynamicAvoidanceModuleManager::init(rclcpp::Node * node) { + // init manager interface + initInterface(node, {""}); + DynamicAvoidanceParameters p{}; { // common - std::string ns = "dynamic_avoidance.common."; + const std::string ns = "dynamic_avoidance.common."; p.enable_debug_info = node->declare_parameter(ns + "enable_debug_info"); p.use_hatched_road_markings = node->declare_parameter(ns + "use_hatched_road_markings"); } { // target object - std::string ns = "dynamic_avoidance.target_object."; + const std::string ns = "dynamic_avoidance.target_object."; p.avoid_car = node->declare_parameter(ns + "car"); p.avoid_truck = node->declare_parameter(ns + "truck"); p.avoid_bus = node->declare_parameter(ns + "bus"); @@ -63,14 +76,20 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( node->declare_parameter(ns + "cut_in_object.min_time_to_start_cut_in"); p.min_lon_offset_ego_to_cut_in_object = node->declare_parameter(ns + "cut_in_object.min_lon_offset_ego_to_object"); + p.min_cut_in_object_vel = node->declare_parameter(ns + "cut_in_object.min_object_vel"); p.max_time_from_outside_ego_path_for_cut_out = node->declare_parameter(ns + "cut_out_object.max_time_from_outside_ego_path"); p.min_cut_out_object_lat_vel = node->declare_parameter(ns + "cut_out_object.min_object_lat_vel"); + p.min_cut_out_object_vel = + node->declare_parameter(ns + "cut_out_object.min_object_vel"); p.max_front_object_angle = node->declare_parameter(ns + "front_object.max_object_angle"); + p.max_front_object_ego_path_lat_cover_ratio = + node->declare_parameter(ns + "front_object.max_ego_path_lat_cover_ratio"); + p.min_front_object_vel = node->declare_parameter(ns + "front_object.min_object_vel"); p.min_overtaking_crossing_object_vel = node->declare_parameter(ns + "crossing_object.min_overtaking_object_vel"); @@ -83,7 +102,9 @@ DynamicAvoidanceModuleManager::DynamicAvoidanceModuleManager( } { // drivable_area_generation - std::string ns = "dynamic_avoidance.drivable_area_generation."; + const std::string ns = "dynamic_avoidance.drivable_area_generation."; + p.polygon_generation_method = convertToPolygonGenerationMethod( + node->declare_parameter(ns + "polygon_generation_method")); p.min_obj_path_based_lon_polygon_margin = node->declare_parameter(ns + "object_path_base.min_longitudinal_polygon_margin"); p.lat_offset_from_obstacle = node->declare_parameter(ns + "lat_offset_from_obstacle"); @@ -157,15 +178,22 @@ void DynamicAvoidanceModuleManager::updateModuleParams( updateParam( parameters, ns + "cut_in_object.min_lon_offset_ego_to_object", p->min_lon_offset_ego_to_cut_in_object); + updateParam(parameters, ns + "cut_in_object.min_object_vel", p->min_cut_in_object_vel); updateParam( parameters, ns + "cut_out_object.max_time_from_outside_ego_path", p->max_time_from_outside_ego_path_for_cut_out); updateParam( parameters, ns + "cut_out_object.min_object_lat_vel", p->min_cut_out_object_lat_vel); + updateParam( + parameters, ns + "cut_out_object.min_object_vel", p->min_cut_out_object_vel); updateParam( parameters, ns + "front_object.max_object_angle", p->max_front_object_angle); + updateParam( + parameters, ns + "front_object.max_ego_path_lat_cover_ratio", + p->max_front_object_ego_path_lat_cover_ratio); + updateParam(parameters, ns + "front_object.min_object_vel", p->min_front_object_vel); updateParam( parameters, ns + "crossing_object.min_overtaking_object_vel", @@ -183,6 +211,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams( { // drivable_area_generation const std::string ns = "dynamic_avoidance.drivable_area_generation."; + std::string polygon_generation_method_str; + if (updateParam( + parameters, ns + "polygon_generation_method", polygon_generation_method_str)) { + p->polygon_generation_method = + convertToPolygonGenerationMethod(polygon_generation_method_str); + } updateParam( parameters, ns + "object_path_base.min_longitudinal_polygon_margin", p->min_obj_path_based_lon_polygon_margin); @@ -222,3 +256,8 @@ void DynamicAvoidanceModuleManager::updateModuleParams( }); } } // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::DynamicAvoidanceModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index ea8b5fd79c5d0..e8905f64e9617 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -14,14 +14,14 @@ #include "behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" @@ -34,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -54,8 +55,10 @@ namespace behavior_path_planner GoalPlannerModule::GoalPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()}, thread_safe_data_{mutex_, clock_} @@ -400,7 +403,7 @@ bool GoalPlannerModule::isExecutionRequested() const bool GoalPlannerModule::isExecutionReady() const { if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { - if (!isSafePath()) { + if (!isSafePath().first) { RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); return false; } @@ -724,7 +727,8 @@ void GoalPlannerModule::setOutput(BehaviorModuleOutput & output) const return; } - if (parameters_->safety_check_params.enable_safety_check && !isSafePath() && isActivated()) { + if ( + parameters_->safety_check_params.enable_safety_check && !isSafePath().first && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -768,7 +772,7 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output) const void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output) const { // safe or not safe(no feasible stop_path found) -> not_safe: try to find new feasible stop_path - if (prev_data_.is_safe || !prev_data_.stop_path_after_approval) { + if (prev_data_.safety_status.is_safe || !prev_data_.stop_path_after_approval) { auto current_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto stop_path = behavior_path_planner::utils::start_goal_planner_common::generateFeasibleStopPath( @@ -850,13 +854,16 @@ void GoalPlannerModule::updateSteeringFactor( // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( - pose, distance, SteeringFactor::GOAL_PLANNER, steering_factor_direction, type, ""); + pose, distance, PlanningBehavior::GOAL_PLANNER, steering_factor_direction, type, ""); } -// NOTE: Once this function returns true, it will continue to return true thereafter. Because -// selectPullOverPath() will not select new path. bool GoalPlannerModule::hasDecidedPath() const { + // Once this function returns true, it will continue to return true thereafter + if (prev_data_.has_decided_path) { + return true; + } + // if path is not safe, not decided if (!thread_safe_data_.foundPullOverPath()) { return false; @@ -864,7 +871,8 @@ bool GoalPlannerModule::hasDecidedPath() const // If it is dangerous before approval, do not determine the path. // This eliminates a unsafe path to be approved - if (parameters_->safety_check_params.enable_safety_check && !isSafePath() && !isActivated()) { + if ( + parameters_->safety_check_params.enable_safety_check && !isSafePath().first && !isActivated()) { return false; } @@ -1031,24 +1039,37 @@ void GoalPlannerModule::updatePreviousData(const BehaviorModuleOutput & output) // for the next loop setOutput(). // this is used to determine whether to generate a new stop path or keep the current stop path. prev_data_.found_path = thread_safe_data_.foundPullOverPath(); - prev_data_.is_safe = parameters_->safety_check_params.enable_safety_check ? isSafePath() : true; - if (!isActivated()) { + prev_data_.has_decided_path = hasDecidedPath(); + + // This is related to safety check, so if it is disabled, return here. + if (!parameters_->safety_check_params.enable_safety_check) { + prev_data_.safety_status.is_safe = true; return; } - if ( - !parameters_->safety_check_params.enable_safety_check || isSafePath() || - (!prev_data_.is_safe && prev_data_.stop_path_after_approval)) { + // Even if the current path is safe, it will not be safe unless it stands for a certain period of + // time. Record the time when the current path has become safe + const auto [is_safe, current_is_safe] = isSafePath(); + if (current_is_safe) { + if (!prev_data_.safety_status.safe_start_time) { + prev_data_.safety_status.safe_start_time = clock_->now(); + } + } else { + prev_data_.safety_status.safe_start_time = std::nullopt; + } + prev_data_.safety_status.is_safe = is_safe; + + // If safety check is enabled, save current path as stop_path_after_approval + // This path is set only once after approval. + if (!isActivated() || (!is_safe && prev_data_.stop_path_after_approval)) { return; } - prev_data_.is_safe = false; - const bool is_stop_path = std::any_of( - output.path->points.begin(), output.path->points.end(), - [](const auto & point) { return point.point.longitudinal_velocity_mps == 0.0; }); - if (is_stop_path) { - prev_data_.stop_path_after_approval = output.path; + auto stop_path = std::make_shared(*output.path); + for (auto & point : stop_path->points) { + point.point.longitudinal_velocity_mps = 0.0; } + prev_data_.stop_path_after_approval = stop_path; } BehaviorModuleOutput GoalPlannerModule::planWaitingApproval() @@ -1135,7 +1156,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const // (In the case of the curve lane, the position is not aligned due to the // difference between the outer and inner sides) // 4. feasible stop - const auto stop_pose = std::invoke([&]() -> boost::optional { + const auto stop_pose = std::invoke([&]() -> std::optional { if (thread_safe_data_.foundPullOverPath()) { return thread_safe_data_.get_pull_over_path()->start_pose; } @@ -1143,7 +1164,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath() const return thread_safe_data_.get_closest_start_pose().value(); } if (!decel_pose) { - return boost::optional{}; + return std::nullopt; } return decel_pose.value(); }); @@ -1645,7 +1666,7 @@ bool GoalPlannerModule::isCrossingPossible( // Lambda function to get the neighboring lanelet based on left_side_parking_ auto getNeighboringLane = - [&](const lanelet::ConstLanelet & lane) -> boost::optional { + [&](const lanelet::ConstLanelet & lane) -> std::optional { lanelet::ConstLanelet neighboring_lane{}; if (left_side_parking_) { if (route_handler->getLeftShoulderLanelet(lane, &neighboring_lane)) { @@ -1678,11 +1699,11 @@ bool GoalPlannerModule::isCrossingPossible( } // Traverse the lanes horizontally until the end_lane_sequence is reached - boost::optional neighboring_lane = getNeighboringLane(current_lane); + std::optional neighboring_lane = getNeighboringLane(current_lane); if (neighboring_lane) { // Check if the neighboring lane is in the end_lane_sequence end_it = - std::find(end_lane_sequence.rbegin(), end_lane_sequence.rend(), neighboring_lane.get()); + std::find(end_lane_sequence.rbegin(), end_lane_sequence.rend(), neighboring_lane.value()); if (end_it != end_lane_sequence.rend()) { return true; } @@ -1722,35 +1743,7 @@ void GoalPlannerModule::updateSafetyCheckTargetObjectsData( goal_planner_data_.ego_predicted_path = ego_predicted_path; } -bool GoalPlannerModule::checkSafetyWithRSS( - const PathWithLaneId & planned_path, - const std::vector & ego_predicted_path, - const std::vector & objects, const double hysteresis_factor) const -{ - // Check for collisions with each predicted path of the object - const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { - auto current_debug_data = marker_utils::createObjectDebug(object); - - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - object, objects_filtering_params_->check_all_predicted_path); - - return std::any_of( - obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { - const bool has_collision = !utils::path_safety_checker::checkCollision( - planned_path, ego_predicted_path, object, obj_path, planner_data_->parameters, - safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second); - - marker_utils::updateCollisionCheckDebugMap( - goal_planner_data_.collision_check, current_debug_data, !has_collision); - - return has_collision; - }); - }); - - return is_safe; -} - -bool GoalPlannerModule::isSafePath() const +std::pair GoalPlannerModule::isSafePath() const { const auto pull_over_path = thread_safe_data_.get_pull_over_path()->getCurrentPath(); const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -1797,9 +1790,51 @@ bool GoalPlannerModule::isSafePath() const goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); const double hysteresis_factor = - prev_data_.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; - return checkSafetyWithRSS( - pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, hysteresis_factor); + prev_data_.safety_status.is_safe ? 1.0 : parameters_->hysteresis_factor_expand_rate; + + const bool current_is_safe = std::invoke([&]() { + if (parameters_->safety_check_params.method == "RSS") { + return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( + pull_over_path, ego_predicted_path, target_objects_on_lane.on_current_lane, + goal_planner_data_.collision_check, planner_data_->parameters, + safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, + hysteresis_factor); + } else if (parameters_->safety_check_params.method == "integral_predicted_polygon") { + return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( + ego_predicted_path, vehicle_info_, target_objects_on_lane.on_current_lane, + objects_filtering_params_->check_all_predicted_path, + parameters_->safety_check_params.integral_predicted_polygon_params, + goal_planner_data_.collision_check); + } + RCLCPP_ERROR( + getLogger(), " [pull_over] invalid safety check method: %s", + parameters_->safety_check_params.method.c_str()); + throw std::domain_error("[pull_over] invalid safety check method"); + }); + + /*　　 + *　　 ==== is_safe + *　　 ---- current_is_safe + *　　 is_safe + *　　 | + *　　 | time + *　　 1 +--+ +---+ +---========= +--+ + *　　 | | | | | | | | + *　　 | | | | | | | | + *　　 | | | | | | | | + *　　 | | | | | | | | + *　　 0 =========================-------==========-- t + */ + if (current_is_safe) { + if ( + prev_data_.safety_status.safe_start_time && + (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds() > + parameters_->safety_check_params.keep_unsafe_time) { + return {true /*is_safe*/, true /*current_is_safe*/}; + } + } + + return {false /*is_safe*/, current_is_safe}; } void GoalPlannerModule::setDebugData() @@ -1897,11 +1932,34 @@ void GoalPlannerModule::setDebugData() goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); } - add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + if (parameters_->safety_check_params.method == "RSS") { + add(showSafetyCheckInfo(goal_planner_data_.collision_check, "object_debug_info")); + } add(showPredictedPath(goal_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(goal_planner_data_.collision_check, "ego_and_target_polygon_relation")); utils::start_goal_planner_common::initializeCollisionCheckDebugMap( goal_planner_data_.collision_check); + + // visualize safety status maker + { + visualization_msgs::msg::MarkerArray marker_array{}; + const auto color = prev_data_.safety_status.is_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "safety_status", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); + + marker.pose = planner_data_->self_odometry->pose.pose; + marker.text += "is_safe: " + std::to_string(prev_data_.safety_status.is_safe) + "\n"; + if (prev_data_.safety_status.safe_start_time) { + const double elapsed_time_from_safe_start = + (clock_->now() - prev_data_.safety_status.safe_start_time.value()).seconds(); + marker.text += + "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; + } + marker_array.markers.push_back(marker); + add(marker_array); + } } // Visualize planner type text diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp index 41d0299bca427..0916b5e8ebaef 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/manager.cpp @@ -25,11 +25,11 @@ namespace behavior_path_planner { - -GoalPlannerModuleManager::GoalPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) -: SceneModuleManagerInterface(node, name, config, {""}) +void GoalPlannerModuleManager::init(rclcpp::Node * node) { + // init manager interface + initInterface(node, {""}); + GoalPlannerParameters p; const std::string base_ns = "goal_planner."; @@ -70,7 +70,8 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( p.parking_policy = ParkingPolicy::RIGHT_SIDE; } else { RCLCPP_ERROR_STREAM( - logger_, "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); + node->get_logger().get_child(name()), + "[goal_planner] invalid parking_policy: " << parking_policy_name << std::endl); exit(EXIT_FAILURE); } } @@ -247,17 +248,15 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(base_ns + "stop_condition.maximum_jerk_for_stop"); } - std::string path_safety_check_ns = "goal_planner.path_safety_check."; + const std::string path_safety_check_ns = "goal_planner.path_safety_check."; // EgoPredictedPath - std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; + const std::string ego_path_ns = path_safety_check_ns + "ego_predicted_path."; { p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.max_velocity = - node->declare_parameter(ego_path_ns + "max_velocity"); + node->declare_parameter(ego_path_ns + "min_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = @@ -269,7 +268,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } // ObjectFilteringParams - std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; + const std::string obj_filter_ns = path_safety_check_ns + "target_filtering."; { p.objects_filtering_params.safety_check_time_horizon = node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); @@ -294,7 +293,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } // ObjectTypesToCheck - std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; { p.objects_filtering_params.object_types_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); @@ -315,7 +314,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } // ObjectLaneConfiguration - std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { p.objects_filtering_params.object_lane_configuration.check_current_lane = node->declare_parameter(obj_lane_ns + "check_current_lane"); @@ -330,10 +329,13 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } // SafetyCheckParams - std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; + const std::string safety_check_ns = path_safety_check_ns + "safety_check_params."; { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); + p.safety_check_params.keep_unsafe_time = + node->declare_parameter(safety_check_ns + "keep_unsafe_time"); + p.safety_check_params.method = node->declare_parameter(safety_check_ns + "method"); p.safety_check_params.hysteresis_factor_expand_rate = node->declare_parameter(safety_check_ns + "hysteresis_factor_expand_rate"); p.safety_check_params.backward_path_length = @@ -345,7 +347,7 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( } // RSSparams - std::string rss_ns = safety_check_ns + "rss_params."; + const std::string rss_ns = safety_check_ns + "rss_params."; { p.safety_check_params.rss_params.rear_vehicle_reaction_time = node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); @@ -359,6 +361,19 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // IntegralPredictedPolygonParams + const std::string integral_ns = safety_check_ns + "integral_predicted_polygon_params."; + { + p.safety_check_params.integral_predicted_polygon_params.forward_margin = + node->declare_parameter(integral_ns + "forward_margin"); + p.safety_check_params.integral_predicted_polygon_params.backward_margin = + node->declare_parameter(integral_ns + "backward_margin"); + p.safety_check_params.integral_predicted_polygon_params.lat_margin = + node->declare_parameter(integral_ns + "lat_margin"); + p.safety_check_params.integral_predicted_polygon_params.time_horizon = + node->declare_parameter(integral_ns + "time_horizon"); + } + // debug { const std::string ns = base_ns + "debug."; @@ -368,16 +383,18 @@ GoalPlannerModuleManager::GoalPlannerModuleManager( // validation of parameters if (p.shift_sampling_num < 1) { RCLCPP_FATAL_STREAM( - logger_, "shift_sampling_num must be positive integer. Given parameter: " - << p.shift_sampling_num << std::endl - << "Terminating the program..."); + node->get_logger().get_child(name()), + "shift_sampling_num must be positive integer. Given parameter: " + << p.shift_sampling_num << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } if (p.maximum_deceleration < 0.0) { RCLCPP_FATAL_STREAM( - logger_, "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); + node->get_logger().get_child(name()), + "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } @@ -391,7 +408,7 @@ void GoalPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] std::string ns = name_ + "."; + [[maybe_unused]] const std::string ns = name_ + "."; std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { if (!observer.expired()) observer.lock()->updateModuleParams(p); @@ -421,7 +438,7 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const return true; } - return enable_simultaneous_execution_as_approved_module_; + return config_.enable_simultaneous_execution_as_approved_module; } bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const @@ -436,7 +453,12 @@ bool GoalPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const return true; } - return enable_simultaneous_execution_as_candidate_module_; + return config_.enable_simultaneous_execution_as_candidate_module; } } // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::GoalPlannerModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp index daba02e1cee89..befd458153e6e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/avoidance_by_lane_change.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/scene_module/lane_change/avoidance_by_lane_change.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index 05b7c5d7dec92..413350e3eb23b 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -15,10 +15,10 @@ #include "behavior_path_planner/scene_module/lane_change/interface.hpp" #include "behavior_path_planner/marker_utils/lane_change/debug.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" #include @@ -36,8 +36,10 @@ using utils::lane_change::assignToCandidate; LaneChangeInterface::LaneChangeInterface( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters, const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map, std::unique_ptr && module_type) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{std::move(parameters)}, module_type_{std::move(module_type)}, prev_approved_path_{std::make_unique()} @@ -108,6 +110,14 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::SUCCESS; } + if (module_type_->isEgoOnPreparePhase() && module_type_->isStoppedAtRedTrafficLight()) { + RCLCPP_WARN_STREAM_THROTTLE( + getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, + "Ego stopped at traffic light. Canceling lane change"); + module_type_->toCancelState(); + return isWaitingApproval() ? ModuleStatus::RUNNING : ModuleStatus::SUCCESS; + } + const auto [is_safe, is_object_coming_from_rear] = module_type_->isApprovedPathSafe(); setObjectDebugVisualization(); @@ -198,6 +208,11 @@ BehaviorModuleOutput LaneChangeInterface::plan() stop_pose_ = module_type_->getStopPose(); + for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -221,6 +236,11 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() module_type_->updateLaneChangeStatus(); setObjectDebugVisualization(); + for (const auto & [uuid, data] : module_type_->getDebugData()) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } + // change turn signal when the vehicle reaches at the end of the path for waiting lane change out.turn_signal_info = getCurrentTurnSignalInfo(*out.path, out.turn_signal_info); @@ -308,27 +328,6 @@ void LaneChangeInterface::setObjectDebugVisualization() const } } -std::shared_ptr LaneChangeInterface::get_debug_msg_array() const -{ - const auto debug_data = module_type_->getDebugData(); - LaneChangeDebugMsgArray debug_msg_array; - debug_msg_array.lane_change_info.reserve(debug_data.size()); - for (const auto & [uuid, debug_data] : debug_data) { - LaneChangeDebugMsg debug_msg; - debug_msg.object_id = uuid; - debug_msg.allow_lane_change = debug_data.is_safe; - debug_msg.is_front = debug_data.is_front; - debug_msg.failed_reason = debug_data.unsafe_reason; - debug_msg.velocity = - std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y); - debug_msg_array.lane_change_info.push_back(debug_msg); - } - lane_change_debug_msg_array_ = debug_msg_array; - - lane_change_debug_msg_array_.header.stamp = clock_->now(); - return std::make_shared(lane_change_debug_msg_array_); -} - MarkerArray LaneChangeInterface::getModuleVirtualWall() { using marker_utils::lane_change_markers::createLaneChangingVirtualWallMarker; @@ -376,7 +375,7 @@ void LaneChangeInterface::updateSteeringFactorPtr(const BehaviorModuleOutput & o // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status.lane_change_path.info.shift_line.start, status.lane_change_path.info.shift_line.end}, - {start_distance, finish_distance}, SteeringFactor::LANE_CHANGE, steering_factor_direction, + {start_distance, finish_distance}, PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::TURNING, ""); } @@ -393,13 +392,7 @@ void LaneChangeInterface::updateSteeringFactorPtr( steering_factor_interface_ptr_->updateSteeringFactor( {selected_path.info.shift_line.start, selected_path.info.shift_line.end}, {output.start_distance_to_path_change, output.finish_distance_to_path_change}, - SteeringFactor::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); -} -void LaneChangeInterface::acceptVisitor(const std::shared_ptr & visitor) const -{ - if (visitor) { - visitor->visitLaneChangeInterface(this); - } + PlanningBehavior::LANE_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); } TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( @@ -482,18 +475,19 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( return original_turn_signal_info; } -void SceneModuleVisitor::visitLaneChangeInterface(const LaneChangeInterface * interface) const -{ - lane_change_visitor_ = interface->get_debug_msg_array(); -} - AvoidanceByLaneChangeInterface::AvoidanceByLaneChangeInterface( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, const std::shared_ptr & avoidance_by_lane_change_parameters, - const std::unordered_map> & rtc_interface_ptr_map) + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) : LaneChangeInterface{ - name, node, parameters, rtc_interface_ptr_map, + name, + node, + parameters, + rtc_interface_ptr_map, + objects_of_interest_marker_interface_ptr_map, std::make_unique(parameters, avoidance_by_lane_change_parameters)} { } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index e772c21331ae8..29b7cad533a66 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -26,16 +26,13 @@ namespace behavior_path_planner { -using route_handler::Direction; -using utils::convertToSnakeCase; - -LaneChangeModuleManager::LaneChangeModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const Direction direction, const LaneChangeModuleType type) -: SceneModuleManagerInterface(node, name, config, {""}), direction_{direction}, type_{type} +void LaneChangeModuleManager::init(rclcpp::Node * node) { using tier4_autoware_utils::getOrDeclareParameter; + // init manager interface + initInterface(node, {""}); + LaneChangeParameters p{}; const auto parameter = [](std::string && name) { return "lane_change." + name; }; @@ -146,7 +143,7 @@ LaneChangeModuleManager::LaneChangeModuleManager( // target object { - std::string ns = "lane_change.target_object."; + const std::string ns = "lane_change.target_object."; p.object_types_to_check.check_car = getOrDeclareParameter(*node, ns + "car"); p.object_types_to_check.check_truck = getOrDeclareParameter(*node, ns + "truck"); p.object_types_to_check.check_bus = getOrDeclareParameter(*node, ns + "bus"); @@ -180,10 +177,11 @@ LaneChangeModuleManager::LaneChangeModuleManager( // validation of parameters if (p.longitudinal_acc_sampling_num < 1 || p.lateral_acc_sampling_num < 1) { RCLCPP_FATAL_STREAM( - logger_, "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " - << p.longitudinal_acc_sampling_num - << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl - << "Terminating the program..."); + node->get_logger().get_child(name()), + "lane_change_sampling_num must be positive integer. Given longitudinal parameter: " + << p.longitudinal_acc_sampling_num + << "Given lateral parameter: " << p.lateral_acc_sampling_num << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } @@ -203,14 +201,17 @@ LaneChangeModuleManager::LaneChangeModuleManager( p.rss_params_for_abort.longitudinal_distance_min_threshold || p.rss_params.longitudinal_velocity_delta_time > p.rss_params_for_abort.longitudinal_velocity_delta_time) { - RCLCPP_FATAL_STREAM(logger_, "abort parameter might be loose... Terminating the program..."); + RCLCPP_FATAL_STREAM( + node->get_logger().get_child(name()), + "abort parameter might be loose... Terminating the program..."); exit(EXIT_FAILURE); } } if (p.cancel.delta_time < 1.0) { RCLCPP_WARN_STREAM( - logger_, "cancel.delta_time: " << p.cancel.delta_time - << ", is too short. This could cause a danger behavior."); + node->get_logger().get_child(name()), + "cancel.delta_time: " << p.cancel.delta_time + << ", is too short. This could cause a danger behavior."); } parameters_ = std::make_shared(p); @@ -221,10 +222,12 @@ std::unique_ptr LaneChangeModuleManager::createNewSceneMod if (type_ == LaneChangeModuleType::NORMAL) { return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, std::make_unique(parameters_, LaneChangeModuleType::NORMAL, direction_)); } return std::make_unique( name_, *node_, parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_, std::make_unique(parameters_, direction_)); } @@ -243,27 +246,21 @@ void LaneChangeModuleManager::updateModuleParams(const std::vector rtc_types = {"left", "right"}; - for (const auto & rtc_type : rtc_types) { - const auto snake_case_name = convertToSnakeCase(name); - const std::string rtc_interface_name = snake_case_name + "_" + rtc_type; - rtc_interface_ptr_map_.emplace( - rtc_type, std::make_shared(node, rtc_interface_name)); - } + // init manager interface + initInterface(node, {"left", "right"}); + + // init lane change manager + LaneChangeModuleManager::init(node); AvoidanceByLCParameters p{}; // unique parameters { - std::string ns = "avoidance_by_lane_change."; + const std::string ns = "avoidance_by_lane_change."; p.execute_object_longitudinal_margin = getOrDeclareParameter(*node, ns + "execute_object_longitudinal_margin"); p.execute_only_when_lane_change_finish_before_object = @@ -272,7 +269,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // general params { - std::string ns = "avoidance."; + const std::string ns = "avoidance."; p.resample_interval_for_planning = getOrDeclareParameter(*node, ns + "resample_interval_for_planning"); p.resample_interval_for_output = @@ -283,7 +280,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.is_target = getOrDeclareParameter(*node, ns + "is_target"); param.execute_num = getOrDeclareParameter(*node, ns + "execute_num"); param.moving_speed_threshold = getOrDeclareParameter(*node, ns + "moving_speed_threshold"); @@ -319,7 +315,24 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // target filtering { - std::string ns = "avoidance.target_filtering."; + const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) { + if (p.object_parameters.count(object_type) == 0) { + return; + } + p.object_parameters.at(object_type).is_avoidance_target = + getOrDeclareParameter(*node, ns); + }; + + const std::string ns = "avoidance.target_filtering."; + set_target_flag(ObjectClassification::CAR, ns + "target_type.car"); + set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck"); + set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer"); + set_target_flag(ObjectClassification::BUS, ns + "target_type.bus"); + set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian"); + set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle"); + set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle"); + set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown"); + p.object_check_goal_distance = getOrDeclareParameter(*node, ns + "object_check_goal_distance"); p.threshold_distance_object_is_on_center = @@ -333,7 +346,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( } { - std::string ns = "avoidance.target_filtering.force_avoidance."; + const std::string ns = "avoidance.target_filtering.force_avoidance."; p.enable_force_avoidance_for_stopped_vehicle = getOrDeclareParameter(*node, ns + "enable"); p.threshold_time_force_avoidance_for_stopped_vehicle = @@ -347,7 +360,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( } { - std::string ns = "avoidance.target_filtering.detection_area."; + const std::string ns = "avoidance.target_filtering.detection_area."; p.use_static_detection_area = getOrDeclareParameter(*node, ns + "static"); p.object_check_min_forward_distance = getOrDeclareParameter(*node, ns + "min_forward_distance"); @@ -359,7 +372,7 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager( // safety check { - std::string ns = "avoidance.safety_check."; + const std::string ns = "avoidance.safety_check."; p.hysteresis_factor_expand_rate = getOrDeclareParameter(*node, ns + "hysteresis_factor_expand_rate"); } @@ -371,7 +384,25 @@ std::unique_ptr AvoidanceByLaneChangeModuleManager::createNewSceneModuleInstance() { return std::make_unique( - name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_); + name_, *node_, parameters_, avoidance_parameters_, rtc_interface_ptr_map_, + objects_of_interest_marker_interface_ptr_map_); } } // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::LaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::LaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeRightModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager, + behavior_path_planner::SceneModuleManagerInterface) +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::AvoidanceByLaneChangeModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index d213eec21cdbf..423ffc48dd6cd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -14,14 +14,14 @@ #include "behavior_path_planner/scene_module/lane_change/normal.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/traffic_light_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -128,6 +128,13 @@ bool NormalLaneChange::isLaneChangeRequired() const return !target_lanes.empty(); } +bool NormalLaneChange::isStoppedAtRedTrafficLight() const +{ + return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + status_.current_lanes, status_.lane_change_path.path, planner_data_, + status_.lane_change_path.info.length.sum()); +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return status_.lane_change_path; @@ -476,7 +483,7 @@ lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const auto backward_length = lane_change_parameters_->backward_lane_length; return route_handler->getLaneletSequence( - lane_change_lane.get(), getEgoPose(), backward_length, forward_length); + lane_change_lane.value(), getEgoPose(), backward_length, forward_length); } bool NormalLaneChange::isNearEndOfCurrentLanes( @@ -1364,9 +1371,16 @@ bool NormalLaneChange::getLaneChangePaths( if ( lane_change_parameters_->regulate_on_traffic_light && !hasEnoughLengthToTrafficLight(*candidate_path, current_lanes)) { + debug_print("Reject: regulate on traffic light!!"); continue; } + if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( + lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + lane_change_info.length.sum())) { + debug_print("Ego is stopping near traffic light. Do not allow lane change"); + continue; + } candidate_paths->push_back(*candidate_path); std::vector filtered_objects = @@ -1718,7 +1732,7 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( lane_change_parameters_->lane_expansion_right_offset); for (const auto & obj : collision_check_objects) { - auto current_debug_data = marker_utils::createObjectDebug(obj); + auto current_debug_data = utils::path_safety_checker::createObjectDebug(obj); const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( obj, lane_change_parameters_->use_all_predicted_path); auto is_safe = true; @@ -1728,7 +1742,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( current_debug_data.second); if (collided_polygons.empty()) { - marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); continue; } @@ -1738,20 +1753,23 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); if (!collision_in_current_lanes && !collision_in_target_lanes) { - marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); continue; } is_safe = false; path_safety_status.is_safe = false; - marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); const auto & obj_pose = obj.initial_pose.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::path_safety_checker::isTargetObjectFront( path, current_pose, common_parameters.vehicle_info, obj_polygon); } - marker_utils::updateCollisionCheckDebugMap(debug_data, current_debug_data, is_safe); + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_data, current_debug_data, is_safe); } return path_safety_status; diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp index 288d9d44aee54..b20712ef19179 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/manager.cpp @@ -25,20 +25,22 @@ namespace behavior_path_planner { -SideShiftModuleManager::SideShiftModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) -: SceneModuleManagerInterface(node, name, config, {}) +void SideShiftModuleManager::init(rclcpp::Node * node) { + // init manager interface + initInterface(node, {}); + SideShiftParameters p{}; + const std::string ns = "side_shift."; p.min_distance_to_start_shifting = - node->declare_parameter(name + ".min_distance_to_start_shifting"); - p.time_to_start_shifting = node->declare_parameter(name + ".time_to_start_shifting"); - p.shifting_lateral_jerk = node->declare_parameter(name + ".shifting_lateral_jerk"); - p.min_shifting_distance = node->declare_parameter(name + ".min_shifting_distance"); - p.min_shifting_speed = node->declare_parameter(name + ".min_shifting_speed"); - p.shift_request_time_limit = node->declare_parameter(name + ".shift_request_time_limit"); - p.publish_debug_marker = node->declare_parameter(name + ".publish_debug_marker"); + node->declare_parameter(ns + "min_distance_to_start_shifting"); + p.time_to_start_shifting = node->declare_parameter(ns + "time_to_start_shifting"); + p.shifting_lateral_jerk = node->declare_parameter(ns + "shifting_lateral_jerk"); + p.min_shifting_distance = node->declare_parameter(ns + "min_shifting_distance"); + p.min_shifting_speed = node->declare_parameter(ns + "min_shifting_speed"); + p.shift_request_time_limit = node->declare_parameter(ns + "shift_request_time_limit"); + p.publish_debug_marker = node->declare_parameter(ns + "publish_debug_marker"); parameters_ = std::make_shared(p); } @@ -50,7 +52,7 @@ void SideShiftModuleManager::updateModuleParams( [[maybe_unused]] auto p = parameters_; - [[maybe_unused]] std::string ns = "side_shift."; + [[maybe_unused]] const std::string ns = "side_shift."; // updateParam(parameters, ns + ..., ...); std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) { @@ -59,3 +61,7 @@ void SideShiftModuleManager::updateModuleParams( } } // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::SideShiftModuleManager, behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index 0e86508ca6afc..9f77288e608cf 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -14,11 +14,11 @@ #include "behavior_path_planner/scene_module/side_shift/side_shift_module.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/side_shift/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include @@ -38,8 +38,11 @@ using tier4_autoware_utils::getPoint; SideShiftModule::SideShiftModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map > & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, parameters_{parameters} + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT + parameters_{parameters} { } @@ -119,7 +122,7 @@ bool SideShiftModule::canTransitSuccessState() const auto isOffsetDiffAlmostZero = [this]() noexcept { const auto last_sp = path_shifter_.getLastShiftLine(); if (last_sp) { - const auto length = std::fabs(last_sp.get().end_shift_length); + const auto length = std::fabs(last_sp.value().end_shift_length); const auto lateral_offset = std::fabs(requested_lateral_offset_); const auto offset_diff = lateral_offset - length; if (!isAlmostZero(offset_diff)) { diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp index fdc28139aa8f0..e379fcfa03a80 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/manager.cpp @@ -24,16 +24,15 @@ namespace behavior_path_planner { - -StartPlannerModuleManager::StartPlannerModuleManager( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config) -: SceneModuleManagerInterface(node, name, config, {""}) +void StartPlannerModuleManager::init(rclcpp::Node * node) { + // init manager interface + initInterface(node, {""}); + StartPlannerParameters p; - std::string ns = "start_planner."; + const std::string ns = "start_planner."; - p.verbose = node->declare_parameter(ns + "verbose"); p.th_arrived_distance = node->declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = node->declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = node->declare_parameter(ns + "th_stopped_time"); @@ -88,7 +87,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "ignore_distance_from_lane_end"); // freespace planner general params { - std::string ns = "start_planner.freespace_planner."; + const std::string ns = "start_planner.freespace_planner."; p.enable_freespace_planner = node->declare_parameter(ns + "enable_freespace_planner"); p.freespace_planner_algorithm = node->declare_parameter(ns + "freespace_planner_algorithm"); @@ -115,7 +114,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // freespace planner search config { - std::string ns = "start_planner.freespace_planner.search_configs."; + const std::string ns = "start_planner.freespace_planner.search_configs."; p.freespace_planner_common_parameters.theta_size = node->declare_parameter(ns + "theta_size"); p.freespace_planner_common_parameters.angle_goal_range = @@ -131,13 +130,13 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // freespace planner costmap configs { - std::string ns = "start_planner.freespace_planner.costmap_configs."; + const std::string ns = "start_planner.freespace_planner.costmap_configs."; p.freespace_planner_common_parameters.obstacle_threshold = node->declare_parameter(ns + "obstacle_threshold"); } // freespace planner astar { - std::string ns = "start_planner.freespace_planner.astar."; + const std::string ns = "start_planner.freespace_planner.astar."; p.astar_parameters.only_behind_solutions = node->declare_parameter(ns + "only_behind_solutions"); p.astar_parameters.use_back = node->declare_parameter(ns + "use_back"); @@ -146,7 +145,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // freespace planner rrtstar { - std::string ns = "start_planner.freespace_planner.rrtstar."; + const std::string ns = "start_planner.freespace_planner.rrtstar."; p.rrt_star_parameters.enable_update = node->declare_parameter(ns + "enable_update"); p.rrt_star_parameters.use_informed_sampling = node->declare_parameter(ns + "use_informed_sampling"); @@ -164,17 +163,15 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(ns + "stop_condition.maximum_jerk_for_stop"); } - std::string base_ns = "start_planner.path_safety_check."; + const std::string base_ns = "start_planner.path_safety_check."; // EgoPredictedPath - std::string ego_path_ns = base_ns + "ego_predicted_path."; + const std::string ego_path_ns = base_ns + "ego_predicted_path."; { p.ego_predicted_path_params.min_velocity = node->declare_parameter(ego_path_ns + "min_velocity"); p.ego_predicted_path_params.acceleration = - node->declare_parameter(ego_path_ns + "acceleration"); - p.ego_predicted_path_params.max_velocity = - node->declare_parameter(ego_path_ns + "max_velocity"); + node->declare_parameter(ego_path_ns + "min_acceleration"); p.ego_predicted_path_params.time_horizon_for_front_object = node->declare_parameter(ego_path_ns + "time_horizon_for_front_object"); p.ego_predicted_path_params.time_horizon_for_rear_object = @@ -186,7 +183,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // ObjectFilteringParams - std::string obj_filter_ns = base_ns + "target_filtering."; + const std::string obj_filter_ns = base_ns + "target_filtering."; { p.objects_filtering_params.safety_check_time_horizon = node->declare_parameter(obj_filter_ns + "safety_check_time_horizon"); @@ -211,7 +208,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // ObjectTypesToCheck - std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; + const std::string obj_types_ns = obj_filter_ns + "object_types_to_check."; { p.objects_filtering_params.object_types_to_check.check_car = node->declare_parameter(obj_types_ns + "check_car"); @@ -232,7 +229,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // ObjectLaneConfiguration - std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; + const std::string obj_lane_ns = obj_filter_ns + "object_lane_configuration."; { p.objects_filtering_params.object_lane_configuration.check_current_lane = node->declare_parameter(obj_lane_ns + "check_current_lane"); @@ -247,7 +244,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // SafetyCheckParams - std::string safety_check_ns = base_ns + "safety_check_params."; + const std::string safety_check_ns = base_ns + "safety_check_params."; { p.safety_check_params.enable_safety_check = node->declare_parameter(safety_check_ns + "enable_safety_check"); @@ -262,7 +259,7 @@ StartPlannerModuleManager::StartPlannerModuleManager( } // RSSparams - std::string rss_ns = safety_check_ns + "rss_params."; + const std::string rss_ns = safety_check_ns + "rss_params."; { p.safety_check_params.rss_params.rear_vehicle_reaction_time = node->declare_parameter(rss_ns + "rear_vehicle_reaction_time"); @@ -276,12 +273,19 @@ StartPlannerModuleManager::StartPlannerModuleManager( node->declare_parameter(rss_ns + "longitudinal_velocity_delta_time"); } + // debug + std::string debug_ns = ns + "debug."; + { + p.print_debug_info = node->declare_parameter(debug_ns + "print_debug_info"); + } + // validation of parameters if (p.lateral_acceleration_sampling_num < 1) { RCLCPP_FATAL_STREAM( - logger_, "lateral_acceleration_sampling_num must be positive integer. Given parameter: " - << p.lateral_acceleration_sampling_num << std::endl - << "Terminating the program..."); + node->get_logger().get_child(name()), + "lateral_acceleration_sampling_num must be positive integer. Given parameter: " + << p.lateral_acceleration_sampling_num << std::endl + << "Terminating the program..."); exit(EXIT_FAILURE); } @@ -295,7 +299,7 @@ void StartPlannerModuleManager::updateModuleParams( auto & p = parameters_; - [[maybe_unused]] std::string ns = name_ + "."; + [[maybe_unused]] const std::string ns = name_ + "."; std::for_each(observers_.begin(), observers_.end(), [&](const auto & observer) { if (!observer.expired()) { @@ -310,12 +314,12 @@ void StartPlannerModuleManager::updateModuleParams( bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const { if (observers_.empty()) { - return enable_simultaneous_execution_as_approved_module_; + return config_.enable_simultaneous_execution_as_approved_module; } const auto checker = [this](const SceneModuleObserver & observer) { if (observer.expired()) { - return enable_simultaneous_execution_as_approved_module_; + return config_.enable_simultaneous_execution_as_approved_module; } const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); @@ -330,7 +334,7 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const return false; } - return enable_simultaneous_execution_as_approved_module_; + return config_.enable_simultaneous_execution_as_approved_module; }; return std::all_of(observers_.begin(), observers_.end(), checker); @@ -339,12 +343,12 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsApprovedModule() const bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() const { if (observers_.empty()) { - return enable_simultaneous_execution_as_candidate_module_; + return config_.enable_simultaneous_execution_as_candidate_module; } const auto checker = [this](const SceneModuleObserver & observer) { if (observer.expired()) { - return enable_simultaneous_execution_as_candidate_module_; + return config_.enable_simultaneous_execution_as_candidate_module; } const auto start_planner_ptr = std::dynamic_pointer_cast(observer.lock()); @@ -359,9 +363,14 @@ bool StartPlannerModuleManager::isSimultaneousExecutableAsCandidateModule() cons return false; } - return enable_simultaneous_execution_as_candidate_module_; + return config_.enable_simultaneous_execution_as_candidate_module; }; return std::all_of(observers_.begin(), observers_.end(), checker); } } // namespace behavior_path_planner + +#include +PLUGINLIB_EXPORT_CLASS( + behavior_path_planner::StartPlannerModuleManager, + behavior_path_planner::SceneModuleManagerInterface) diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 396e39eebba5f..7bbee7fe0a10c 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -14,11 +14,11 @@ #include "behavior_path_planner/scene_module/start_planner/start_planner_module.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -36,16 +36,24 @@ #include #include +using behavior_path_planner::utils::start_goal_planner_common::initializeCollisionCheckDebugMap; using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; +// set as macro so that calling function name will be printed. +// debug print is heavy. turn on only when debugging. +#define DEBUG_PRINT(...) \ + RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__) + namespace behavior_path_planner { StartPlannerModule::StartPlannerModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::unordered_map> & rtc_interface_ptr_map) -: SceneModuleInterface{name, node, rtc_interface_ptr_map}, + const std::unordered_map> & rtc_interface_ptr_map, + std::unordered_map> & + objects_of_interest_marker_interface_ptr_map) +: SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map}, // NOLINT parameters_{parameters}, vehicle_info_{vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo()} { @@ -105,38 +113,59 @@ BehaviorModuleOutput StartPlannerModule::run() void StartPlannerModule::processOnEntry() { - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - start_planner_data_.collision_check); - } + initVariables(); } void StartPlannerModule::processOnExit() +{ + initVariables(); +} + +void StartPlannerModule::initVariables() { resetPathCandidate(); resetPathReference(); debug_marker_.markers.clear(); + initializeSafetyCheckParameters(); + initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } void StartPlannerModule::updateData() { - if (isBackwardDrivingComplete()) { + if (receivedNewRoute()) { + resetStatus(); + DEBUG_PRINT("StartPlannerModule::updateData() received new route, reset status"); + } + + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); + DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving"); } else { status_.backward_driving_complete = false; } - if (receivedNewRoute()) { - status_ = PullOutStatus(); + if (requiresDynamicObjectsCollisionDetection()) { + status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects(); } - // check safety status when driving forward - if (parameters_->safety_check_params.enable_safety_check && status_.driving_forward) { - status_.is_safe_dynamic_objects = isSafePath(); - } else { - status_.is_safe_dynamic_objects = true; +} + +bool StartPlannerModule::hasFinishedBackwardDriving() const +{ + // check ego car is close enough to pull out start pose and stopped + const auto current_pose = planner_data_->self_odometry->pose.pose; + const auto distance = + tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); + + const bool is_near = distance < parameters_->th_arrived_distance; + const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); + const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; + + const bool back_finished = !status_.driving_forward && is_near && is_stopped; + if (back_finished) { + RCLCPP_INFO(getLogger(), "back finished"); } + + return back_finished; } bool StartPlannerModule::receivedNewRoute() const @@ -145,6 +174,18 @@ bool StartPlannerModule::receivedNewRoute() const *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); } +bool StartPlannerModule::requiresDynamicObjectsCollisionDetection() const +{ + return parameters_->safety_check_params.enable_safety_check && status_.driving_forward; +} + +bool StartPlannerModule::hasCollisionWithDynamicObjects() const +{ + // TODO(Sugahara): update path, params for predicted path and so on in this function to avoid + // mutable + return !isSafePath(); +} + bool StartPlannerModule::isExecutionRequested() const { if (isModuleRunning()) { @@ -209,25 +250,42 @@ bool StartPlannerModule::isMoving() const parameters_->th_stopped_velocity; } +bool StartPlannerModule::isStopped() +{ + odometry_buffer_.push_back(planner_data_->self_odometry); + // Delete old data in buffer + while (rclcpp::ok()) { + const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - + rclcpp::Time(odometry_buffer_.front()->header.stamp); + if (time_diff.seconds() < parameters_->th_stopped_time) { + break; + } + odometry_buffer_.pop_front(); + } + return !std::any_of( + odometry_buffer_.begin(), odometry_buffer_.end(), [this](const auto & odometry) { + return utils::l2Norm(odometry->twist.twist.linear) > parameters_->th_stopped_velocity; + }); +} + bool StartPlannerModule::isExecutionReady() const { - // when found_pull_out_path is false,the path is not generated and approval shouldn't be - // allowed + bool is_safe = true; + + // Evaluate safety. The situation is not safe if any of the following conditions are met: + // 1. pull out path has not been found + // 2. waiting for approval and there is a collision with dynamic objects if (!status_.found_pull_out_path) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Pull over path is not found"); - return false; + is_safe = false; } - if ( - parameters_->safety_check_params.enable_safety_check && status_.driving_forward && - isWaitingApproval()) { - if (!isSafePath()) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects"); - stop_pose_ = planner_data_->self_odometry->pose.pose; - return false; - } + if (requiresDynamicObjectsCollisionDetection()) { + is_safe = !hasCollisionWithDynamicObjects(); } - return true; + + if (!is_safe) stop_pose_ = planner_data_->self_odometry->pose.pose; + + return is_safe; } bool StartPlannerModule::canTransitSuccessState() @@ -327,7 +385,7 @@ BehaviorModuleOutput StartPlannerModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, + {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } else { const double distance = motion_utils::calcSignedArcLength( @@ -337,7 +395,7 @@ BehaviorModuleOutput StartPlannerModule::plan() // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, ""); } setDebugData(); @@ -440,7 +498,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() updateRTCStatus(start_distance, finish_distance); steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, - {start_distance, finish_distance}, SteeringFactor::START_PLANNER, steering_factor_direction, + {start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } else { const double distance = motion_utils::calcSignedArcLength( @@ -449,7 +507,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() updateRTCStatus(0.0, distance); steering_factor_interface_ptr_->updateSteeringFactor( {status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance}, - SteeringFactor::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); + PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, ""); } setDebugData(); @@ -459,8 +517,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { - PullOutStatus initial_status; - status_ = initial_status; + status_ = PullOutStatus{}; } void StartPlannerModule::incrementPathIndex() @@ -720,7 +777,7 @@ void StartPlannerModule::updatePullOutStatus() const auto pull_out_lanes = start_planner_utils::getPullOutLanes( planner_data_, planner_data_->parameters.backward_path_length + parameters_->max_back_distance); - if (isBackwardDrivingComplete()) { + if (hasFinishedBackwardDriving()) { updateStatusAfterBackwardDriving(); } else { status_.backward_path = start_planner_utils::getBackwardPath( @@ -864,48 +921,6 @@ bool StartPlannerModule::hasFinishedPullOut() const return has_finished; } -bool StartPlannerModule::isBackwardDrivingComplete() const -{ - // check ego car is close enough to pull out start pose and stopped - const auto current_pose = planner_data_->self_odometry->pose.pose; - const auto distance = - tier4_autoware_utils::calcDistance2d(current_pose, status_.pull_out_start_pose); - - const bool is_near = distance < parameters_->th_arrived_distance; - const double ego_vel = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear); - const bool is_stopped = ego_vel < parameters_->th_stopped_velocity; - - const bool back_finished = !status_.driving_forward && is_near && is_stopped; - if (back_finished) { - RCLCPP_INFO(getLogger(), "back finished"); - } - - return back_finished; -} - -bool StartPlannerModule::isStopped() -{ - odometry_buffer_.push_back(planner_data_->self_odometry); - // Delete old data in buffer - while (rclcpp::ok()) { - const auto time_diff = rclcpp::Time(odometry_buffer_.back()->header.stamp) - - rclcpp::Time(odometry_buffer_.front()->header.stamp); - if (time_diff.seconds() < parameters_->th_stopped_time) { - break; - } - odometry_buffer_.pop_front(); - } - bool is_stopped = true; - for (const auto & odometry : odometry_buffer_) { - const double ego_vel = utils::l2Norm(odometry->twist.twist.linear); - if (ego_vel > parameters_->th_stopped_velocity) { - is_stopped = false; - break; - } - } - return is_stopped; -} - bool StartPlannerModule::isStuck() { if (!isStopped()) { @@ -1089,35 +1104,11 @@ bool StartPlannerModule::isSafePath() const utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData( start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path); - bool is_safe_dynamic_objects = true; - // Check for collisions with each predicted path of the object - for (const auto & object : target_objects_on_lane.on_current_lane) { - auto current_debug_data = marker_utils::createObjectDebug(object); - - bool is_safe_dynamic_object = true; - - const auto obj_predicted_paths = utils::path_safety_checker::getPredictedPathFromObj( - object, objects_filtering_params_->check_all_predicted_path); - - // If a collision is detected, mark the object as unsafe and break the loop - for (const auto & obj_path : obj_predicted_paths) { - if (!utils::path_safety_checker::checkCollision( - pull_out_path, ego_predicted_path, object, obj_path, planner_data_->parameters, - safety_check_params_->rss_params, hysteresis_factor, current_debug_data.second)) { - marker_utils::updateCollisionCheckDebugMap( - start_planner_data_.collision_check, current_debug_data, false); - is_safe_dynamic_objects = false; - is_safe_dynamic_object = false; - break; - } - } - if (is_safe_dynamic_object) { - marker_utils::updateCollisionCheckDebugMap( - start_planner_data_.collision_check, current_debug_data, is_safe_dynamic_object); - } - } - - return is_safe_dynamic_objects; + return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( + pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane, + start_planner_data_.collision_check, planner_data_->parameters, + safety_check_params_->rss_params, objects_filtering_params_->use_all_predicted_path, + hysteresis_factor); } bool StartPlannerModule::isGoalBehindOfEgoInSameRouteSegment() const @@ -1284,8 +1275,7 @@ void StartPlannerModule::setDebugData() const add(showSafetyCheckInfo(start_planner_data_.collision_check, "object_debug_info")); add(showPredictedPath(start_planner_data_.collision_check, "ego_predicted_path")); add(showPolygon(start_planner_data_.collision_check, "ego_and_target_polygon_relation")); - utils::start_goal_planner_common::initializeCollisionCheckDebugMap( - start_planner_data_.collision_check); + initializeCollisionCheckDebugMap(start_planner_data_.collision_check); } // Visualize planner type text @@ -1308,9 +1298,6 @@ void StartPlannerModule::setDebugData() const planner_type_marker_array.markers.push_back(marker); add(planner_type_marker_array); } - if (parameters_->verbose) { - logPullOutStatus(); - } } void StartPlannerModule::logPullOutStatus(rclcpp::Logger::Level log_level) const diff --git a/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp new file mode 100644 index 0000000000000..c7fbaa931737c --- /dev/null +++ b/planning/behavior_path_planner/src/utils/avoidance/shift_line_generator.cpp @@ -0,0 +1,1330 @@ +// Copyright 2023 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "behavior_path_planner/utils/avoidance/shift_line_generator.hpp" + +#include "behavior_path_planner/utils/avoidance/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" + +#include + +#include + +namespace behavior_path_planner::utils::avoidance +{ + +using tier4_autoware_utils::generateUUID; +using tier4_autoware_utils::getPoint; +using tier4_planning_msgs::msg::AvoidanceDebugFactor; + +namespace +{ +bool isBestEffort(const std::string & policy) +{ + return policy == "best_effort"; +} + +bool perManeuver(const std::string & policy) +{ + return policy == "per_avoidance_maneuver"; +} + +AvoidLine merge(const AvoidLine & line1, const AvoidLine & line2, const UUID id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.start_idx; + ret.start_shift_length = line1.start_shift_length; + ret.start_longitudinal = line1.start_longitudinal; + + ret.end_idx = line2.end_idx; + ret.end_shift_length = line2.end_shift_length; + ret.end_longitudinal = line2.end_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLine fill(const AvoidLine & line1, const AvoidLine & line2, const UUID id) +{ + AvoidLine ret{}; + + ret.start_idx = line1.end_idx; + ret.start_shift_length = line1.end_shift_length; + ret.start_longitudinal = line1.end_longitudinal; + + ret.end_idx = line2.start_idx; + ret.end_shift_length = line2.start_shift_length; + ret.end_longitudinal = line2.start_longitudinal; + + ret.id = id; + ret.object = line1.object; + + return ret; +} + +AvoidLineArray toArray(const AvoidOutlines & outlines) +{ + AvoidLineArray ret{}; + for (const auto & outline : outlines) { + ret.push_back(outline.avoid_line); + ret.push_back(outline.return_line); + + std::for_each( + outline.middle_lines.begin(), outline.middle_lines.end(), + [&ret](const auto & line) { ret.push_back(line); }); + } + return ret; +} +} // namespace + +void ShiftLineGenerator::update(AvoidancePlanningData & data, DebugData & debug) +{ + /** + * STEP1: Update registered shift line. + */ + updateRegisteredRawShiftLines(data); + + /** + * STEP2: Generate avoid outlines. + * Basically, avoid outlines are generated per target objects. + */ + const auto outlines = generateAvoidOutline(data, debug); + + /** + * STEP3: Create rough shift lines. + */ + raw_ = applyPreProcess(outlines, data, debug); +} + +AvoidLineArray ShiftLineGenerator::generate( + const AvoidancePlanningData & data, DebugData & debug) const +{ + return generateCandidateShiftLine(raw_, data, debug); +} + +AvoidOutlines ShiftLineGenerator::generateAvoidOutline( + AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug) const +{ + // To be consistent with changes in the ego position, the current shift length is considered. + const auto current_ego_shift = helper_->getEgoShift(); + const auto & base_link2rear = data_->parameters.base_link2rear; + + // Calculate feasible shift length + const auto get_shift_profile = + [&]( + auto & object, const auto & desire_shift_length) -> std::optional> { + // use each object param + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto is_object_on_right = utils::avoidance::isOnRight(object); + + // use absolute dist for return-to-center, relative dist from current for avoiding. + const auto avoiding_shift = desire_shift_length - current_ego_shift; + const auto nominal_avoid_distance = helper_->getMaxAvoidanceDistance(avoiding_shift); + + // calculate remaining distance. + const auto prepare_distance = helper_->getNominalPrepareDistance(); + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front + : 0.0; + const auto constant = object_parameter.safety_buffer_longitudinal + + additional_buffer_longitudinal + prepare_distance; + const auto has_enough_distance = object.longitudinal > constant + nominal_avoid_distance; + const auto remaining_distance = object.longitudinal - constant; + const auto avoidance_distance = + has_enough_distance ? nominal_avoid_distance : remaining_distance; + + // nominal case. avoidable. + if (has_enough_distance) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + if (!isBestEffort(parameters_->policy_lateral_margin)) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // ego already has enough positive shift. + const auto has_enough_positive_shift = avoiding_shift < -1e-3 && desire_shift_length > 1e-3; + if (is_object_on_right && has_enough_positive_shift) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // ego already has enough negative shift. + const auto has_enough_negative_shift = avoiding_shift > 1e-3 && desire_shift_length < -1e-3; + if (!is_object_on_right && has_enough_negative_shift) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // don't relax shift length since it can stop in front of the object. + if (object.is_stoppable && !parameters_->use_shorten_margin_immediately) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // the avoidance path is already approved + const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position; + const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) || + (helper_->getShift(object_pos) < 0.0 && !is_object_on_right); + if (is_approved) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // prepare distance is not enough. unavoidable. + if (remaining_distance < 1e-3) { + object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO; + return std::nullopt; + } + + // calculate lateral jerk. + const auto required_jerk = PathShifter::calcJerkFromLatLonDistance( + avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed()); + + // relax lateral jerk limit. avoidable. + if (required_jerk < helper_->getLateralMaxJerkLimit()) { + return std::make_pair(desire_shift_length, avoidance_distance); + } + + // avoidance distance is not enough. unavoidable. + if (!isBestEffort(parameters_->policy_deceleration)) { + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } + + // output avoidance path under lateral jerk constraints. + const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk( + remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed()); + + if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return std::nullopt; + } + + const auto feasible_shift_length = + desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift + : -1.0 * feasible_relative_shift_length + current_ego_shift; + + const auto infeasible = + std::abs(feasible_shift_length - object.overhang_dist) < + 0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral; + if (infeasible) { + RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. "); + object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK; + return std::nullopt; + } + + return std::make_pair(feasible_shift_length, avoidance_distance); + }; + + const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; }; + + const auto is_valid_shift_line = [](const auto & s) { + return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal; + }; + + AvoidOutlines outlines; + for (auto & o : data.target_objects) { + if (!o.avoid_margin.has_value()) { + o.reason = AvoidanceDebugFactor::INSUFFICIENT_LATERAL_MARGIN; + if (o.avoid_required && is_forward_object(o)) { + break; + } else { + continue; + } + } + + const auto is_object_on_right = utils::avoidance::isOnRight(o); + const auto desire_shift_length = + helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value()); + if (utils::avoidance::isSameDirectionShift(is_object_on_right, desire_shift_length)) { + o.reason = AvoidanceDebugFactor::SAME_DIRECTION_SHIFT; + if (o.avoid_required && is_forward_object(o)) { + break; + } else { + continue; + } + } + + // use each object param + const auto object_type = utils::getHighestProbLabel(o.object.classification); + const auto object_parameter = parameters_->object_parameters.at(object_type); + const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length); + + if (!feasible_shift_profile.has_value()) { + if (o.avoid_required && is_forward_object(o)) { + break; + } else { + continue; + } + } + + // use absolute dist for return-to-center, relative dist from current for avoiding. + const auto feasible_return_distance = + helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first); + + AvoidLine al_avoid; + { + const auto & additional_buffer_longitudinal = + object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front + : 0.0; + const auto offset = + object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal; + const auto to_shift_end = o.longitudinal - offset; + const auto path_front_to_ego = data.arclength_from_ego.at(data.ego_closest_path_index); + + // start point (use previous linear shift length as start shift length.) + al_avoid.start_longitudinal = [&]() { + const auto nearest_avoid_distance = + std::max(to_shift_end - feasible_shift_profile.value().second, 1e-3); + + if (data.to_start_point > to_shift_end) { + return nearest_avoid_distance; + } + + const auto minimum_avoid_distance = helper_->getMinAvoidanceDistance( + feasible_shift_profile.value().first - current_ego_shift); + const auto furthest_avoid_distance = std::max(to_shift_end - minimum_avoid_distance, 1e-3); + + return std::clamp(data.to_start_point, nearest_avoid_distance, furthest_avoid_distance); + }(); + + al_avoid.start_idx = utils::avoidance::findPathIndexFromArclength( + data.arclength_from_ego, al_avoid.start_longitudinal + path_front_to_ego); + al_avoid.start = data.reference_path.points.at(al_avoid.start_idx).point.pose; + al_avoid.start_shift_length = helper_->getLinearShift(al_avoid.start.position); + + // end point + al_avoid.end_shift_length = feasible_shift_profile.value().first; + al_avoid.end_longitudinal = to_shift_end; + + // misc + al_avoid.id = generateUUID(); + al_avoid.object = o; + al_avoid.object_on_right = utils::avoidance::isOnRight(o); + } + + AvoidLine al_return; + { + const auto offset = object_parameter.safety_buffer_longitudinal + base_link2rear + o.length; + const auto to_shift_start = o.longitudinal + offset; + + // start point + al_return.start_shift_length = feasible_shift_profile.value().first; + al_return.start_longitudinal = to_shift_start; + + // end point + al_return.end_longitudinal = [&]() { + if (data.to_return_point > to_shift_start) { + return std::clamp( + data.to_return_point, to_shift_start, feasible_return_distance + to_shift_start); + } + + return to_shift_start + feasible_return_distance; + }(); + al_return.end_shift_length = 0.0; + + // misc + al_return.id = generateUUID(); + al_return.object = o; + al_return.object_on_right = utils::avoidance::isOnRight(o); + } + + if (is_valid_shift_line(al_avoid) && is_valid_shift_line(al_return)) { + outlines.emplace_back(al_avoid, al_return); + } else { + o.reason = "InvalidShiftLine"; + continue; + } + + o.is_avoidable = true; + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, outlines); + + debug.step1_current_shift_line = toArray(outlines); + + return outlines; +} + +AvoidLineArray ShiftLineGenerator::applyPreProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidOutlines processed_outlines = outlines; + + /** + * Step1: Rough merge process. + * Merge multiple avoid outlines. If an avoid outlines' return shift line conflicts other + * outline's avoid shift line, those avoid outlines are merged. + */ + processed_outlines = applyMergeProcess(processed_outlines, data, debug); + + /** + * Step2: Fill gap process. + * Create and add new shift line to avoid outline in order to fill gaps between avoid shift line + * and middle shift lines, return shift line and middle shift lines. + */ + processed_outlines = applyFillGapProcess(processed_outlines, data, debug); + + /** + * Step3: Convert to AvoidLineArray from AvoidOutlines. + */ + AvoidLineArray processed_raw_lines = toArray(processed_outlines); + + /** + * Step4: Combine process. + * Use all registered points. For the current points, if the similar one of the current + * points are already registered, will not use it. + */ + processed_raw_lines = applyCombineProcess(processed_raw_lines, raw_registered_, debug); + + /* + * Step5: Add return shift line. + * Add return-to-center shift point from the last shift point, if needed. + * If there is no shift points, set return-to center shift from ego. + */ + processed_raw_lines = addReturnShiftLine(processed_raw_lines, data, debug); + + /* + * Step6: Fill gap process. + * Create and add new shift line to avoid lines. + */ + return applyFillGapProcess(processed_raw_lines, data, debug); +} + +AvoidLineArray ShiftLineGenerator::generateCandidateShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidLineArray processed_shift_lines = shift_lines; + + /** + * Step1: Merge process. + * Merge positive shift avoid lines and negative shift avoid lines. + */ + processed_shift_lines = applyMergeProcess(processed_shift_lines, data, debug); + + /** + * Step2: Clean up process. + * Remove noisy shift line and concat same gradient shift lines. + */ + processed_shift_lines = applyTrimProcess(processed_shift_lines, debug); + + /** + * Step3: Extract new shift lines. + * Compare processed shift lines and registered shift lines in order to find new shift lines. + */ + return findNewShiftLine(processed_shift_lines, debug); +} + +void ShiftLineGenerator::generateTotalShiftLine( + const AvoidLineArray & avoid_lines, const AvoidancePlanningData & data, + ShiftLineData & shift_line_data) const +{ + const auto & path = data.reference_path; + const auto & arcs = data.arclength_from_ego; + const auto N = path.points.size(); + + auto & sl = shift_line_data; + + sl.shift_line = std::vector(N, 0.0); + sl.shift_line_grad = std::vector(N, 0.0); + + sl.pos_shift_line = std::vector(N, 0.0); + sl.neg_shift_line = std::vector(N, 0.0); + + sl.pos_shift_line_grad = std::vector(N, 0.0); + sl.neg_shift_line_grad = std::vector(N, 0.0); + + // debug + sl.shift_line_history = std::vector>(avoid_lines.size(), sl.shift_line); + + // take minmax for same directional shift length + for (size_t j = 0; j < avoid_lines.size(); ++j) { + const auto & al = avoid_lines.at(j); + for (size_t i = 0; i < N; ++i) { + // calc current interpolated shift + const auto i_shift = utils::avoidance::lerpShiftLengthOnArc(arcs.at(i), al); + + // update maximum shift for positive direction + if (i_shift > sl.pos_shift_line.at(i)) { + sl.pos_shift_line.at(i) = i_shift; + sl.pos_shift_line_grad.at(i) = al.getGradient(); + } + + // update minumum shift for negative direction + if (i_shift < sl.neg_shift_line.at(i)) { + sl.neg_shift_line.at(i) = i_shift; + sl.neg_shift_line_grad.at(i) = al.getGradient(); + } + + // store for debug print + sl.shift_line_history.at(j).at(i) = i_shift; + } + } + + // Merge shift length of opposite directions. + for (size_t i = 0; i < N; ++i) { + sl.shift_line.at(i) = sl.pos_shift_line.at(i) + sl.neg_shift_line.at(i); + sl.shift_line_grad.at(i) = sl.pos_shift_line_grad.at(i) + sl.neg_shift_line_grad.at(i); + } + + // overwrite shift with current_ego_shift until ego pose. + const auto current_shift = helper_->getEgoLinearShift(); + for (size_t i = 0; i < sl.shift_line.size(); ++i) { + if (data.ego_closest_path_index < i) { + break; + } + sl.shift_line.at(i) = current_shift; + sl.shift_line_grad.at(i) = 0.0; + } + + // If the shift point does not have an associated object, + // use previous value. + for (size_t i = 1; i < N; ++i) { + bool has_object = false; + for (const auto & al : avoid_lines) { + if (al.start_idx <= i && i <= al.end_idx) { + has_object = true; + break; + } + } + if (!has_object) { + sl.shift_line.at(i) = sl.shift_line.at(i - 1); + } + } + + if (avoid_lines.empty()) { + sl.shift_line_history.push_back(sl.shift_line); + return; + } + + const auto grad_first_shift_line = (avoid_lines.front().start_shift_length - current_shift) / + avoid_lines.front().start_longitudinal; + + for (size_t i = data.ego_closest_path_index; i <= avoid_lines.front().start_idx; ++i) { + sl.shift_line.at(i) = helper_->getLinearShift(getPoint(path.points.at(i))); + sl.shift_line_grad.at(i) = grad_first_shift_line; + } + + sl.shift_line_history.push_back(sl.shift_line); +} + +AvoidLineArray ShiftLineGenerator::extractShiftLinesFromLine( + const AvoidancePlanningData & data, ShiftLineData & shift_line_data) const +{ + using utils::avoidance::setEndData; + using utils::avoidance::setStartData; + + const auto & path = data.reference_path; + const auto & arcs = data.arclength_from_ego; + const auto N = path.points.size(); + + auto & sl = shift_line_data; + + const auto backward_grad = [&](const size_t i) { + if (i == 0) { + return sl.shift_line_grad.at(i); + } + const double ds = arcs.at(i) - arcs.at(i - 1); + if (ds < 1.0e-5) { + return sl.shift_line_grad.at(i); + } // use theoretical value when ds is too small. + return (sl.shift_line.at(i) - sl.shift_line.at(i - 1)) / ds; + }; + + const auto forward_grad = [&](const size_t i) { + if (i == arcs.size() - 1) { + return sl.shift_line_grad.at(i); + } + const double ds = arcs.at(i + 1) - arcs.at(i); + if (ds < 1.0e-5) { + return sl.shift_line_grad.at(i); + } // use theoretical value when ds is too small. + return (sl.shift_line.at(i + 1) - sl.shift_line.at(i)) / ds; + }; + + // calculate forward and backward gradient of the shift length. + // This will be used for grad-change-point check. + sl.forward_grad = std::vector(N, 0.0); + sl.backward_grad = std::vector(N, 0.0); + for (size_t i = 0; i < N - 1; ++i) { + sl.forward_grad.at(i) = forward_grad(i); + sl.backward_grad.at(i) = backward_grad(i); + } + + AvoidLineArray merged_avoid_lines; + AvoidLine al{}; + bool found_first_start = false; + constexpr auto CREATE_SHIFT_GRAD_THR = 0.001; + constexpr auto IS_ALREADY_SHIFTING_THR = 0.001; + for (size_t i = data.ego_closest_path_index; i < N - 1; ++i) { + const auto & p = path.points.at(i).point.pose; + const auto shift = sl.shift_line.at(i); + + // If the vehicle is already on the avoidance (checked by the first point has shift), + // set a start point at the first path point. + if (!found_first_start && std::abs(shift) > IS_ALREADY_SHIFTING_THR) { + setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + found_first_start = true; + } + + // find the point where the gradient of the shift is changed + const bool set_shift_line_flag = + std::abs(sl.forward_grad.at(i) - sl.backward_grad.at(i)) > CREATE_SHIFT_GRAD_THR; + + if (!set_shift_line_flag) { + continue; + } + + if (!found_first_start) { + setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + found_first_start = true; + } else { + setEndData(al, shift, p, i, arcs.at(i)); + al.id = generateUUID(); + merged_avoid_lines.push_back(al); + setStartData(al, 0.0, p, i, arcs.at(i)); // start length is overwritten later. + } + } + + if (std::abs(backward_grad(N - 1)) > CREATE_SHIFT_GRAD_THR) { + const auto & p = path.points.at(N - 1).point.pose; + const auto shift = sl.shift_line.at(N - 1); + setEndData(al, shift, p, N - 1, arcs.at(N - 1)); + al.id = generateUUID(); + merged_avoid_lines.push_back(al); + } + + return merged_avoid_lines; +} + +AvoidOutlines ShiftLineGenerator::applyMergeProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidOutlines ret{}; + + if (outlines.size() < 2) { + return outlines; + } + + const auto no_conflict = [](const auto & line1, const auto & line2) { + return line1.end_idx < line2.start_idx || line2.end_idx < line1.start_idx; + }; + + const auto same_side_shift = [](const auto & line1, const auto & line2) { + return line1.object_on_right == line2.object_on_right; + }; + + const auto within = [](const auto & line, const size_t idx) { + return line.start_idx < idx && idx < line.end_idx; + }; + + ret.push_back(outlines.front()); + + for (size_t i = 1; i < outlines.size(); i++) { + auto & last_outline = ret.back(); + auto & next_outline = outlines.at(i); + + const auto & return_line = last_outline.return_line; + const auto & avoid_line = next_outline.avoid_line; + + if (no_conflict(return_line, avoid_line)) { + ret.push_back(outlines.at(i)); + continue; + } + + const auto merged_shift_line = merge(return_line, avoid_line, generateUUID()); + + if (!helper_->isComfortable(AvoidLineArray{merged_shift_line})) { + ret.push_back(outlines.at(i)); + continue; + } + + if (same_side_shift(return_line, avoid_line)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.end_idx) && within(avoid_line, return_line.start_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + + if (within(return_line, avoid_line.start_idx) && within(avoid_line, return_line.end_idx)) { + last_outline.middle_lines.push_back(merged_shift_line); + last_outline.return_line = next_outline.return_line; + debug.step1_merged_shift_line.push_back(merged_shift_line); + continue; + } + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_merged_shift_line); + + return ret; +} + +AvoidOutlines ShiftLineGenerator::applyFillGapProcess( + const AvoidOutlines & outlines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidOutlines ret = outlines; + + for (auto & outline : ret) { + if (outline.middle_lines.empty()) { + const auto new_line = fill(outline.avoid_line, outline.return_line, generateUUID()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } + + helper_->alignShiftLinesOrder(outline.middle_lines, false); + + if (outline.avoid_line.end_longitudinal < outline.middle_lines.front().start_longitudinal) { + const auto new_line = fill(outline.avoid_line, outline.middle_lines.front(), generateUUID()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } + + helper_->alignShiftLinesOrder(outline.middle_lines, false); + + if (outline.middle_lines.back().end_longitudinal < outline.return_line.start_longitudinal) { + const auto new_line = fill(outline.middle_lines.back(), outline.return_line, generateUUID()); + outline.middle_lines.push_back(new_line); + debug.step1_filled_shift_line.push_back(new_line); + } + + helper_->alignShiftLinesOrder(outline.middle_lines, false); + } + + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_filled_shift_line); + + return ret; +} + +AvoidLineArray ShiftLineGenerator::applyFillGapProcess( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidLineArray sorted = shift_lines; + + helper_->alignShiftLinesOrder(sorted, false); + + AvoidLineArray ret = sorted; + + if (shift_lines.empty()) { + return ret; + } + + // fill gap between ego and nearest shift line. + if (sorted.front().start_longitudinal > 0.0) { + AvoidLine ego_line{}; + utils::avoidance::setEndData( + ego_line, helper_->getEgoLinearShift(), data.reference_pose, data.ego_closest_path_index, + 0.0); + + const auto new_line = fill(ego_line, sorted.front(), generateUUID()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); + } + + helper_->alignShiftLinesOrder(sorted, false); + + // fill gap among shift lines. + for (size_t i = 0; i < sorted.size() - 1; ++i) { + if (sorted.at(i + 1).start_longitudinal < sorted.at(i).end_longitudinal) { + continue; + } + + const auto new_line = fill(sorted.at(i), sorted.at(i + 1), generateUUID()); + ret.push_back(new_line); + debug.step1_front_shift_line.push_back(new_line); + } + + helper_->alignShiftLinesOrder(ret, false); + + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, ret); + utils::avoidance::fillAdditionalInfoFromLongitudinal(data, debug.step1_front_shift_line); + + return ret; +} + +AvoidLineArray ShiftLineGenerator::applyCombineProcess( + const AvoidLineArray & shift_lines, const AvoidLineArray & registered_lines, + DebugData & debug) const +{ + debug.step1_registered_shift_line = registered_lines; + return utils::avoidance::combineRawShiftLinesWithUniqueCheck(registered_lines, shift_lines); +} + +AvoidLineArray ShiftLineGenerator::applyMergeProcess( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const +{ + // Generate shift line by merging shift_lines. + ShiftLineData shift_line_data; + generateTotalShiftLine(shift_lines, data, shift_line_data); + + // Re-generate shift points by detecting gradient-change point of the shift line. + auto merged_shift_lines = extractShiftLinesFromLine(data, shift_line_data); + + // set parent id + for (auto & al : merged_shift_lines) { + al.parent_ids = utils::avoidance::calcParentIds(shift_lines, al); + } + + // sort by distance from ego. + helper_->alignShiftLinesOrder(merged_shift_lines); + + // debug visualize + { + debug.pos_shift = shift_line_data.pos_shift_line; + debug.neg_shift = shift_line_data.neg_shift_line; + debug.total_shift = shift_line_data.shift_line; + debug.pos_shift_grad = shift_line_data.pos_shift_line_grad; + debug.neg_shift_grad = shift_line_data.neg_shift_line_grad; + debug.total_forward_grad = shift_line_data.forward_grad; + debug.total_backward_grad = shift_line_data.backward_grad; + debug.step2_merged_shift_line = merged_shift_lines; + } + + return merged_shift_lines; +} + +AvoidLineArray ShiftLineGenerator::applyTrimProcess( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + if (shift_lines.empty()) { + return shift_lines; + } + + AvoidLineArray sl_array_trimmed = shift_lines; + + // sort shift points from front to back. + helper_->alignShiftLinesOrder(sl_array_trimmed); + + // - Change the shift length to the previous one if the deviation is small. + { + constexpr double SHIFT_DIFF_THRES = 1.0; + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); + } + + // - Combine avoid points that have almost same gradient. + // this is to remove the noise. + { + const auto THRESHOLD = parameters_->same_grad_filter_1_threshold; + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_1st = sl_array_trimmed; + } + + // - Quantize the shift length to reduce the shift point noise + // This is to remove the noise coming from detection accuracy, interpolation, resampling, etc. + { + const auto THRESHOLD = parameters_->quantize_filter_threshold; + applyQuantizeProcess(sl_array_trimmed, THRESHOLD); + debug.step3_quantize_filtered = sl_array_trimmed; + } + + // - Change the shift length to the previous one if the deviation is small. + { + constexpr double SHIFT_DIFF_THRES = 1.0; + applySmallShiftFilter(sl_array_trimmed, SHIFT_DIFF_THRES); + debug.step3_noise_filtered = sl_array_trimmed; + } + + // - Combine avoid points that have almost same gradient (again) + { + const auto THRESHOLD = parameters_->same_grad_filter_2_threshold; + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_2nd = sl_array_trimmed; + } + + // - Combine avoid points that have almost same gradient (again) + { + const auto THRESHOLD = parameters_->same_grad_filter_3_threshold; + applySimilarGradFilter(sl_array_trimmed, THRESHOLD); + debug.step3_grad_filtered_3rd = sl_array_trimmed; + } + + return sl_array_trimmed; +} + +void ShiftLineGenerator::applyQuantizeProcess( + AvoidLineArray & shift_lines, const double threshold) const +{ + if (threshold < 1.0e-5) { + return; // no need to process + } + + for (auto & sl : shift_lines) { + sl.end_shift_length = std::round(sl.end_shift_length / threshold) * threshold; + } + + helper_->alignShiftLinesOrder(shift_lines); +} + +void ShiftLineGenerator::applySmallShiftFilter( + AvoidLineArray & shift_lines, const double threshold) const +{ + if (shift_lines.empty()) { + return; + } + + AvoidLineArray input = shift_lines; + shift_lines.clear(); + + for (const auto & s : input) { + if (s.getRelativeLongitudinal() < threshold) { + continue; + } + + if (s.start_longitudinal < helper_->getMinimumPrepareDistance()) { + continue; + } + + shift_lines.push_back(s); + } +} + +void ShiftLineGenerator::applySimilarGradFilter( + AvoidLineArray & avoid_lines, const double threshold) const +{ + if (avoid_lines.empty()) { + return; + } + + AvoidLineArray input = avoid_lines; + avoid_lines.clear(); + avoid_lines.push_back(input.front()); // Take the first one anyway (think later) + + AvoidLine base_line = input.front(); + + AvoidLineArray combine_buffer; + combine_buffer.push_back(input.front()); + + constexpr auto SHIFT_THR = 1e-3; + const auto is_negative_shift = [&](const auto & s) { + return s.getRelativeLength() < -1.0 * SHIFT_THR; + }; + + const auto is_positive_shift = [&](const auto & s) { return s.getRelativeLength() > SHIFT_THR; }; + + for (size_t i = 1; i < input.size(); ++i) { + AvoidLine combine{}; + + utils::avoidance::setStartData( + combine, base_line.start_shift_length, base_line.start, base_line.start_idx, + base_line.start_longitudinal); + utils::avoidance::setEndData( + combine, input.at(i).end_shift_length, input.at(i).end, input.at(i).end_idx, + input.at(i).end_longitudinal); + + combine_buffer.push_back(input.at(i)); + + const auto violates = [&]() { + if (is_negative_shift(input.at(i)) && is_positive_shift(base_line)) { + return true; + } + + if (is_negative_shift(base_line) && is_positive_shift(input.at(i))) { + return true; + } + + const auto lon_combine = combine.getRelativeLongitudinal(); + const auto base_length = base_line.getGradient() * lon_combine; + return std::abs(combine.getRelativeLength() - base_length) > threshold; + }(); + + if (violates) { + avoid_lines.push_back(input.at(i)); + base_line = input.at(i); + combine_buffer.clear(); + combine_buffer.push_back(input.at(i)); + } else { + avoid_lines.back() = combine; + } + } + + helper_->alignShiftLinesOrder(avoid_lines); +} + +AvoidLineArray ShiftLineGenerator::addReturnShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data, DebugData & debug) const +{ + AvoidLineArray ret = shift_lines; + + constexpr double ep = 1.0e-3; + const bool has_candidate_point = !ret.empty(); + const bool has_registered_point = last_.has_value(); + + const auto exist_unavoidable_object = std::any_of( + data.target_objects.begin(), data.target_objects.end(), + [](const auto & o) { return !o.is_avoidable && o.longitudinal > 0.0; }); + + if (exist_unavoidable_object) { + return ret; + } + + // If the return-to-center shift points are already registered, do nothing. + if (!has_registered_point && std::fabs(base_offset_) < ep) { + return ret; + } + + constexpr double RETURN_SHIFT_THRESHOLD = 0.1; + if (std::abs(last_.value().end_shift_length) < RETURN_SHIFT_THRESHOLD) { + return ret; + } + + // From here, the return-to-center is not registered. But perhaps the candidate is + // already generated. + + // If it has a shift point, add return shift from the existing last shift point. + // If not, add return shift from ego point. (prepare distance is considered for both.) + ShiftLine last_sl; // the return-shift will be generated after the last shift point. + { + // avoidance points: Yes, shift points: No -> select last avoidance point. + if (has_candidate_point && !has_registered_point) { + helper_->alignShiftLinesOrder(ret, false); + last_sl = ret.back(); + } + + // avoidance points: No, shift points: Yes -> select last shift point. + if (!has_candidate_point && has_registered_point) { + last_sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + } + + // avoidance points: Yes, shift points: Yes -> select the last one from both. + if (has_candidate_point && has_registered_point) { + helper_->alignShiftLinesOrder(ret, false); + const auto & al = ret.back(); + const auto & sl = utils::avoidance::fillAdditionalInfo(data, AvoidLine{last_.value()}); + last_sl = (sl.end_longitudinal > al.end_longitudinal) ? sl : al; + } + + // avoidance points: No, shift points: No -> set the ego position to the last shift point + // so that the return-shift will be generated from ego position. + if (!has_candidate_point && !has_registered_point) { + last_sl.end_idx = data.ego_closest_path_index; + last_sl.end = data.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_shift_length = base_offset_; + } + } + + // There already is a shift point candidates to go back to center line, but it could be too sharp + // due to detection noise or timing. + // Here the return-shift from ego is added for the in case. + if (std::fabs(last_sl.end_shift_length) < RETURN_SHIFT_THRESHOLD) { + const auto current_base_shift = helper_->getEgoShift(); + if (std::abs(current_base_shift) < ep) { + return ret; + } + + // Is there a shift point in the opposite direction of the current_base_shift? + // No -> we can overwrite the return shift, because the other shift points that decrease + // the shift length are for return-shift. + // Yes -> we can NOT overwrite, because it might be not a return-shift, but a avoiding + // shift to the opposite direction which can not be overwritten by the return-shift. + for (const auto & sl : ret) { + if ( + (current_base_shift > 0.0 && sl.end_shift_length < -ep) || + (current_base_shift < 0.0 && sl.end_shift_length > ep)) { + return ret; + } + } + + // If return shift already exists in candidate or registered shift lines, skip adding return + // shift. + if (has_candidate_point || has_registered_point) { + return ret; + } + + // set the return-shift from ego. + last_sl.end_idx = data.ego_closest_path_index; + last_sl.end = data.reference_path.points.at(last_sl.end_idx).point.pose; + last_sl.end_shift_length = current_base_shift; + } + + const auto & arclength_from_ego = data.arclength_from_ego; + + const auto nominal_prepare_distance = helper_->getNominalPrepareDistance(); + const auto nominal_avoid_distance = helper_->getMaxAvoidanceDistance(last_sl.end_shift_length); + + if (arclength_from_ego.empty()) { + return ret; + } + + const auto remaining_distance = std::min( + arclength_from_ego.back() - parameters_->dead_line_buffer_for_goal, data.to_return_point); + + // If the avoidance point has already been set, the return shift must be set after the point. + const auto last_sl_distance = data.arclength_from_ego.at(last_sl.end_idx); + + // check if there is enough distance for return. + if (last_sl_distance > remaining_distance) { // tmp: add some small number (+1.0) + RCLCPP_DEBUG(rclcpp::get_logger(""), "No enough distance for return."); + return ret; + } + + // If the remaining distance is not enough, the return shift needs to be shrunk. + // (or another option is just to ignore the return-shift.) + // But we do not want to change the last shift point, so we will shrink the distance after + // the last shift point. + // + // The line "===" is fixed, "---" is scaled. + // + // [Before Scaling] + // ego last_sl_end prepare_end path_end avoid_end + // ==o====================o----------------------o----------------------o------------o + // | prepare_dist | avoid_dist | + // + // [After Scaling] + // ==o====================o------------------o--------------------------o + // | prepare_dist_scaled | avoid_dist_scaled | + // + const double variable_prepare_distance = + std::max(nominal_prepare_distance - last_sl_distance, 0.0); + + double prepare_distance_scaled = std::max(nominal_prepare_distance, last_sl_distance); + double avoid_distance_scaled = nominal_avoid_distance; + if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) { + const auto scale = (remaining_distance - last_sl_distance) / + std::max(nominal_avoid_distance + variable_prepare_distance, 0.1); + prepare_distance_scaled = last_sl_distance + scale * nominal_prepare_distance; + avoid_distance_scaled *= scale; + } + + // shift point for prepare distance: from last shift to return-start point. + if (nominal_prepare_distance > last_sl_distance) { + AvoidLine al; + al.id = generateUUID(); + al.start_idx = last_sl.end_idx; + al.start = last_sl.end; + al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.end_idx = + utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.end = data.reference_path.points.at(al.end_idx).point.pose; + al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_shift_length = last_sl.end_shift_length; + al.start_shift_length = last_sl.end_shift_length; + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); + } + + // shift point for return to center line + { + AvoidLine al; + al.id = generateUUID(); + al.start_idx = + utils::avoidance::findPathIndexFromArclength(arclength_from_ego, prepare_distance_scaled); + al.start = data.reference_path.points.at(al.start_idx).point.pose; + al.start_longitudinal = arclength_from_ego.at(al.start_idx); + al.end_idx = utils::avoidance::findPathIndexFromArclength( + arclength_from_ego, prepare_distance_scaled + avoid_distance_scaled); + al.end = data.reference_path.points.at(al.end_idx).point.pose; + al.end_longitudinal = arclength_from_ego.at(al.end_idx); + al.end_shift_length = 0.0; + al.start_shift_length = last_sl.end_shift_length; + ret.push_back(al); + debug.step1_return_shift_line.push_back(al); + } + + return ret; +} + +AvoidLineArray ShiftLineGenerator::findNewShiftLine( + const AvoidLineArray & shift_lines, DebugData & debug) const +{ + if (shift_lines.empty()) { + return {}; + } + + // add small shift lines. + const auto add_straight_shift = + [&, this](auto & subsequent, bool has_large_shift, const size_t start_idx) { + for (size_t i = start_idx; i < shift_lines.size(); ++i) { + if ( + std::abs(shift_lines.at(i).getRelativeLength()) > + parameters_->lateral_small_shift_threshold) { + if (has_large_shift) { + return; + } + + has_large_shift = true; + } + + if (!helper_->isComfortable(AvoidLineArray{shift_lines.at(i)})) { + return; + } + + subsequent.push_back(shift_lines.at(i)); + } + }; + + // get subsequent shift lines. + const auto get_subsequent_shift = [&, this](size_t i) { + AvoidLineArray subsequent{shift_lines.at(i)}; + + if (!helper_->isComfortable(subsequent)) { + return subsequent; + } + + if (shift_lines.size() == i + 1) { + return subsequent; + } + + if (!helper_->isComfortable(AvoidLineArray{shift_lines.at(i + 1)})) { + return subsequent; + } + + if ( + std::abs(shift_lines.at(i).getRelativeLength()) < + parameters_->lateral_small_shift_threshold) { + const auto has_large_shift = + shift_lines.at(i + 1).getRelativeLength() > parameters_->lateral_small_shift_threshold; + + // candidate.at(i) is small length shift line. add large length shift line. + subsequent.push_back(shift_lines.at(i + 1)); + add_straight_shift(subsequent, has_large_shift, i + 2); + } else { + // candidate.at(i) is large length shift line. add small length shift lines. + add_straight_shift(subsequent, true, i + 1); + } + + return subsequent; + }; + + // check ignore or not. + const auto is_ignore_shift = [this](const auto & s) { + return std::abs(helper_->getRelativeShiftToPath(s)) < parameters_->lateral_execution_threshold; + }; + + for (size_t i = 0; i < shift_lines.size(); ++i) { + const auto & candidate = shift_lines.at(i); + + // prevent sudden steering. + if (candidate.start_longitudinal < helper_->getMinimumPrepareDistance()) { + break; + } + + if (is_ignore_shift(candidate)) { + continue; + } + + if (perManeuver(parameters_->policy_approval)) { + debug.step4_new_shift_line = shift_lines; + return shift_lines; + } + + const auto new_shift_lines = get_subsequent_shift(i); + debug.step4_new_shift_line = new_shift_lines; + return new_shift_lines; + } + + return {}; +} + +void ShiftLineGenerator::updateRegisteredRawShiftLines(const AvoidancePlanningData & data) +{ + utils::avoidance::fillAdditionalInfoFromPoint(data, raw_registered_); + + AvoidLineArray avoid_lines; + + const auto has_large_offset = [this](const auto & s) { + constexpr double THRESHOLD = 0.1; + const auto ego_shift_length = helper_->getEgoLinearShift(); + + const auto start_to_ego_longitudinal = -1.0 * s.start_longitudinal; + + if (start_to_ego_longitudinal < 0.0) { + return false; + } + + const auto reg_shift_length = + s.getGradient() * start_to_ego_longitudinal + s.start_shift_length; + + return std::abs(ego_shift_length - reg_shift_length) > THRESHOLD; + }; + + const auto ego_idx = data.ego_closest_path_index; + + for (const auto & s : raw_registered_) { + // invalid + if (s.end_idx < ego_idx) { + continue; + } + + // invalid + if (has_large_offset(s)) { + continue; + } + + // valid + avoid_lines.push_back(s); + } + + raw_registered_ = avoid_lines; +} + +void ShiftLineGenerator::setRawRegisteredShiftLine( + const AvoidLineArray & shift_lines, const AvoidancePlanningData & data) +{ + if (shift_lines.empty()) { + RCLCPP_ERROR(rclcpp::get_logger(""), "future is empty! return."); + return; + } + + auto future_with_info = shift_lines; + utils::avoidance::fillAdditionalInfoFromPoint(data, future_with_info); + + // sort by longitudinal + std::sort(future_with_info.begin(), future_with_info.end(), [](auto a, auto b) { + return a.end_longitudinal < b.end_longitudinal; + }); + + // calc relative lateral length + future_with_info.front().start_shift_length = base_offset_; + for (size_t i = 1; i < future_with_info.size(); ++i) { + future_with_info.at(i).start_shift_length = future_with_info.at(i - 1).end_shift_length; + } + + const auto is_registered = [this](const auto id) { + const auto & r = raw_registered_; + return std::any_of(r.begin(), r.end(), [id](const auto & s) { return s.id == id; }); + }; + + const auto same_id_shift_line = [this](const auto id) { + const auto & r = raw_; + const auto itr = std::find_if(r.begin(), r.end(), [id](const auto & s) { return s.id == id; }); + if (itr != r.end()) { + return *itr; + } + throw std::logic_error("not found same id current raw shift line."); + }; + + for (const auto & s : future_with_info) { + if (s.parent_ids.empty()) { + RCLCPP_ERROR(rclcpp::get_logger(""), "avoid line for path_shifter must have parent_id."); + } + + for (const auto id : s.parent_ids) { + if (is_registered(id)) { + continue; + } + + raw_registered_.push_back(same_id_shift_line(id)); + } + } +} +} // namespace behavior_path_planner::utils::avoidance diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index ce4c9ad7ac72d..6fd1943369cb9 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "behavior_path_planner/utils/avoidance/avoidance_module_data.hpp" #include "behavior_path_planner/utils/avoidance/utils.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/traffic_light_utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/traffic_light_utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include #include #include #include +#include #include #include @@ -232,12 +233,21 @@ void pushUniqueVector(T & base_vector, const T & additional_vector) } // namespace -bool isOnRight(const ObjectData & obj) +namespace filtering_utils { - return obj.lateral < 0.0; +bool isAvoidanceTargetObjectType( + const PredictedObject & object, const std::shared_ptr & parameters) +{ + const auto object_type = utils::getHighestProbLabel(object.classification); + + if (parameters->object_parameters.count(object_type) == 0) { + return false; + } + + return parameters->object_parameters.at(object_type).is_avoidance_target; } -bool isTargetObjectType( +bool isSafetyCheckTargetObjectType( const PredictedObject & object, const std::shared_ptr & parameters) { const auto object_type = utils::getHighestProbLabel(object.classification); @@ -246,7 +256,7 @@ bool isTargetObjectType( return false; } - return parameters->object_parameters.at(object_type).is_target; + return parameters->object_parameters.at(object_type).is_safety_check_target; } bool isVehicleTypeObject(const ObjectData & object) @@ -313,8 +323,132 @@ bool isWithinIntersection( object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon()))); } +bool isParallelToEgoLane(const ObjectData & object, const double threshold) +{ + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto closest_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose)); + + return yaw_deviation < threshold || yaw_deviation > M_PI - threshold; +} + +bool isObjectOnRoadShoulder( + ObjectData & object, const std::shared_ptr & route_handler, + const std::shared_ptr & parameters) +{ + using boost::geometry::return_centroid; + using boost::geometry::within; + using lanelet::geometry::distance2d; + using lanelet::geometry::toArcCoordinates; + using lanelet::utils::to2D; + + // assume that there are no parked vehicles in intersection. + std::string turn_direction = object.overhang_lanelet.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + return false; + } + + // ============================================ <- most_left_lanelet.leftBound() + // y road shoulder + // ^ ------------------------------------------ + // | x + + // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() + // + // -------------------------------------------- + // +: object position + // o: nearest point on centerline + + lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); + + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto centerline_pose = + lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position); + lanelet::BasicPoint3d centerline_point( + centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); + + bool is_left_side_parked_vehicle = false; + if (!isOnRight(object)) { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_left_road_lanelet = + route_handler->getMostLeftLanelet(object.overhang_lanelet); + const auto most_left_lanelet_candidates = + route_handler->getLaneletMapPtr()->laneletLayer.findUsages( + most_left_road_lanelet.leftBound()); + + lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; + const lanelet::Attribute sub_type = + most_left_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_left_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_left_lanelet = ll; + } + } + + const auto center_to_left_boundary = + distance2d(to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_left_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; + + is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + bool is_right_side_parked_vehicle = false; + if (isOnRight(object)) { + auto [object_shiftable_distance, sub_type] = [&]() { + const auto most_right_road_lanelet = + route_handler->getMostRightLanelet(object.overhang_lanelet); + const auto most_right_lanelet_candidates = + route_handler->getLaneletMapPtr()->laneletLayer.findUsages( + most_right_road_lanelet.rightBound()); + + lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; + const lanelet::Attribute sub_type = + most_right_lanelet.attribute(lanelet::AttributeName::Subtype); + + for (const auto & ll : most_right_lanelet_candidates) { + const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); + if (sub_type.value() == "road_shoulder") { + most_right_lanelet = ll; + } + } + + const auto center_to_right_boundary = + distance2d(to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); + + return std::make_pair( + center_to_right_boundary - 0.5 * object.object.shape.dimensions.y, sub_type); + }(); + + if (sub_type.value() != "road_shoulder") { + object_shiftable_distance += parameters->object_check_min_road_shoulder_width; + } + + const auto arc_coordinates = toArcCoordinates( + to2D(object.overhang_lanelet.centerline().basicLineString()), object_centroid); + object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; + + is_right_side_parked_vehicle = + object.shiftable_ratio > parameters->object_check_shiftable_ratio; + } + + return is_left_side_parked_vehicle || is_right_side_parked_vehicle; +} + bool isForceAvoidanceTarget( - ObjectData & object, const lanelet::ConstLanelets & extend_lanelets, + ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { @@ -350,7 +484,7 @@ bool isForceAvoidanceTarget( bool not_parked_object = true; // check traffic light - const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, extend_lanelets); + const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets); { not_parked_object = to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; @@ -358,7 +492,7 @@ bool isForceAvoidanceTarget( // check crosswalk const auto to_crosswalk = - utils::getDistanceToCrosswalk(ego_pose, extend_lanelets, *rh->getOverallGraphPtr()) - + utils::getDistanceToCrosswalk(ego_pose, data.extend_lanelets, *rh->getOverallGraphPtr()) - object.longitudinal; { const auto stop_for_crosswalk = @@ -372,6 +506,181 @@ bool isForceAvoidanceTarget( return !not_parked_object; } +bool isSatisfiedWithCommonCondition( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + // Step1. filtered by target object type. + if (!isAvoidanceTargetObjectType(object.object, parameters)) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; + return false; + } + + // Step2. filtered stopped objects. + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + if (object.move_time > object_parameter.moving_time_threshold) { + object.reason = AvoidanceDebugFactor::MOVING_OBJECT; + return false; + } + + // Step3. filtered by longitudinal distance. + const auto & ego_pos = planner_data->self_odometry->pose.pose.position; + fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, object); + + if (object.longitudinal < -parameters->object_check_backward_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; + return false; + } + + if (object.longitudinal > parameters->object_check_max_forward_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; + return false; + } + + // Step4. filtered by distance between object and goal position. + // TODO(Satoshi OTA): remove following two conditions after it can execute avoidance and goal + // planner module simultaneously. + const auto & rh = planner_data->route_handler; + const auto ego_idx = planner_data->findEgoIndex(data.reference_path_rough.points); + const auto to_goal_distance = + rh->isInGoalRouteSection(data.current_lanelets.back()) + ? calcSignedArcLength( + data.reference_path_rough.points, ego_idx, data.reference_path_rough.points.size() - 1) + : std::numeric_limits::max(); + + if (object.longitudinal > to_goal_distance) { + object.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; + return false; + } + + if ( + object.longitudinal + object.length / 2 + parameters->object_check_goal_distance > + to_goal_distance) { + object.reason = "TooNearToGoal"; + return false; + } + + return true; +} + +bool isSatisfiedWithNonVehicleCondition( + ObjectData & object, [[maybe_unused]] const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + [[maybe_unused]] const std::shared_ptr & parameters) +{ + // avoidance module ignore pedestrian and bicycle around crosswalk + if (isWithinCrosswalk(object, planner_data->route_handler->getOverallGraphPtr())) { + object.reason = "CrosswalkUser"; + return false; + } + + return true; +} + +bool isSatisfiedWithVehicleCondition( + ObjectData & object, const AvoidancePlanningData & data, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + using boost::geometry::within; + + object.is_within_intersection = isWithinIntersection(object, planner_data->route_handler); + + // from here condition check for vehicle type objects. + if (isForceAvoidanceTarget(object, data, planner_data, parameters)) { + return true; + } + + // Object is on center line -> ignore. + if (std::abs(object.lateral) < parameters->threshold_distance_object_is_on_center) { + object.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + return false; + } + + lanelet::BasicPoint2d object_centroid(object.centroid.x(), object.centroid.y()); + const auto on_ego_driving_lane = + within(object_centroid, object.overhang_lanelet.polygon2d().basicPolygon()); + if (on_ego_driving_lane) { + if (isObjectOnRoadShoulder(object, planner_data->route_handler, parameters)) { + return true; + } else { + object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + return false; + } + } + + if (!object.is_within_intersection) { + return true; + } + + if (isParallelToEgoLane(object, parameters->object_check_yaw_deviation)) { + object.reason = "ParallelToEgoLane"; + return false; + } + + return true; +} + +bool isNoNeedAvoidanceBehavior( + ObjectData & object, const std::shared_ptr & parameters) +{ + if (!object.avoid_margin.has_value()) { + return false; + } + + const auto shift_length = + calcShiftLength(isOnRight(object), object.overhang_dist, object.avoid_margin.value()); + if (!isShiftNecessary(isOnRight(object), shift_length)) { + object.reason = "NotNeedAvoidance"; + return true; + } + + if (std::abs(shift_length) < parameters->lateral_execution_threshold) { + object.reason = "LessThanExecutionThreshold"; + return true; + } + + return false; +} + +std::optional getAvoidMargin( + const ObjectData & object, const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + const auto & vehicle_width = planner_data->parameters.vehicle_width; + const auto object_type = utils::getHighestProbLabel(object.object.classification); + const auto object_parameter = parameters->object_parameters.at(object_type); + + const auto max_avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor + + object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; + const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; + const auto soft_lateral_distance_limit = + object.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; + const auto hard_lateral_distance_limit = + object.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; + + // Step1. check avoidable or not. + if (hard_lateral_distance_limit < min_avoid_margin) { + return std::nullopt; + } + + // Step2. check if it should expand road shoulder margin. + if (soft_lateral_distance_limit < min_avoid_margin) { + return min_avoid_margin; + } + + // Step3. nominal case. avoid margin is limited by soft constraint. + return std::min(soft_lateral_distance_limit, max_avoid_margin); +} +} // namespace filtering_utils + +bool isOnRight(const ObjectData & obj) +{ + return obj.lateral < 0.0; +} + double calcShiftLength( const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin) { @@ -444,22 +753,24 @@ size_t findPathIndexFromArclength( return path_arclength_arr.size() - 1; } -std::vector concatParentIds( - const std::vector & ids1, const std::vector & ids2) +std::vector concatParentIds(const std::vector & ids1, const std::vector & ids2) { - std::set id_set{ids1.begin(), ids1.end()}; - for (const auto id : ids2) { - id_set.insert(id); + std::vector ret; + for (const auto id2 : ids2) { + if (std::any_of(ids1.begin(), ids1.end(), [&id2](const auto & id1) { return id1 == id2; })) { + continue; + } + ret.push_back(id2); } - const auto v = std::vector{id_set.begin(), id_set.end()}; - return v; + + return ret; } -std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2) +std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine & lines2) { // Get the ID of the original AP whose transition area overlaps with the given AP, // and set it to the parent id. - std::set ids; + std::vector ret; for (const auto & al : lines1) { const auto p_s = al.start_longitudinal; const auto p_e = al.end_longitudinal; @@ -469,9 +780,9 @@ std::vector calcParentIds(const AvoidLineArray & lines1, const AvoidLine continue; } - ids.insert(al.id); + ret.push_back(al.id); } - return std::vector(ids.begin(), ids.end()); + return ret; } double lerpShiftLengthOnArc(double arc, const AvoidLine & ap) @@ -627,7 +938,7 @@ std::vector generateObstaclePolygonsForDrivableArea( } // check if avoid marin is calculated - if (!object.avoid_margin) { + if (!object.avoid_margin.has_value()) { continue; } @@ -636,7 +947,7 @@ std::vector generateObstaclePolygonsForDrivableArea( // generate obstacle polygon const double diff_poly_buffer = - object.avoid_margin.get() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; + object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0; const auto obj_poly = tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); const bool is_left = 0 < object.lateral; @@ -664,14 +975,14 @@ lanelet::ConstLanelets getTargetLanelets( const auto opt_left_lane = rh->getLeftLanelet(lane); if (opt_left_lane) { - target_lanelets.push_back(opt_left_lane.get()); + target_lanelets.push_back(opt_left_lane.value()); } else { l_offset = left_offset; } const auto opt_right_lane = rh->getRightLanelet(lane); if (opt_right_lane) { - target_lanelets.push_back(opt_right_lane.get()); + target_lanelets.push_back(opt_right_lane.value()); } else { r_offset = right_offset; } @@ -707,9 +1018,36 @@ lanelet::ConstLanelets getCurrentLanesFromPath( start_lane, p.backward_path_length, p.forward_path_length); } +lanelet::ConstLanelets getExtendLanes( + const lanelet::ConstLanelets & lanelets, const Pose & ego_pose, + const std::shared_ptr & planner_data) +{ + lanelet::ConstLanelets extend_lanelets = lanelets; + + while (rclcpp::ok()) { + const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets); + const auto arc_coordinates = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose); + const auto forward_length = lane_length - arc_coordinates.length; + + if (forward_length > planner_data->parameters.forward_path_length) { + break; + } + + const auto next_lanelets = planner_data->route_handler->getNextLanelets(extend_lanelets.back()); + + if (next_lanelets.empty()) { + break; + } + + extend_lanelets.push_back(next_lanelets.front()); + } + + return extend_lanelets; +} + void insertDecelPoint( const Point & p_src, const double offset, const double velocity, PathWithLaneId & path, - boost::optional & p_out) + std::optional & p_out) { const auto decel_point = calcLongitudinalOffsetPoint(path.points, p_src, offset); @@ -718,8 +1056,8 @@ void insertDecelPoint( return; } - const auto seg_idx = findNearestSegmentIndex(path.points, decel_point.get()); - const auto insert_idx = insertTargetPoint(seg_idx, decel_point.get(), path.points); + const auto seg_idx = findNearestSegmentIndex(path.points, decel_point.value()); + const auto insert_idx = insertTargetPoint(seg_idx, decel_point.value(), path.points); if (!insert_idx) { // TODO(Satoshi OTA) Think later the process in the case of no decel point found. @@ -727,7 +1065,7 @@ void insertDecelPoint( } const auto insertVelocity = [&insert_idx](PathWithLaneId & path, const float v) { - for (size_t i = insert_idx.get(); i < path.points.size(); ++i) { + for (size_t i = insert_idx.value(); i < path.points.size(); ++i) { const auto & original_velocity = path.points.at(i).point.longitudinal_velocity_mps; path.points.at(i).point.longitudinal_velocity_mps = std::min(original_velocity, v); } @@ -735,7 +1073,7 @@ void insertDecelPoint( insertVelocity(path, velocity); - p_out = getPose(path.points.at(insert_idx.get())); + p_out = getPose(path.points.at(insert_idx.value())); } void fillObjectEnvelopePolygon( @@ -1007,361 +1345,123 @@ void compensateDetectionLost( } } -void filterTargetObjects( - ObjectDataArray & objects, AvoidancePlanningData & data, DebugData & debug, +double getRoadShoulderDistance( + ObjectData & object, const AvoidancePlanningData & data, const std::shared_ptr & planner_data, const std::shared_ptr & parameters) { - using boost::geometry::return_centroid; - using boost::geometry::within; - using lanelet::geometry::distance2d; - using lanelet::geometry::toArcCoordinates; using lanelet::utils::to2D; - if (data.current_lanelets.empty()) { - return; - } - - const auto & rh = planner_data->route_handler; - const auto & path_points = data.reference_path_rough.points; - const auto & ego_pos = planner_data->self_odometry->pose.pose.position; - const auto & vehicle_width = planner_data->parameters.vehicle_width; - const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); - - // for goal - const auto ego_idx = planner_data->findEgoIndex(path_points); - const auto dist_to_goal = rh->isInGoalRouteSection(data.current_lanelets.back()) - ? calcSignedArcLength(path_points, ego_idx, path_points.size() - 1) - : std::numeric_limits::max(); - - // extend lanelets if the reference path is cut for lane change. - const auto & ego_pose = planner_data->self_odometry->pose.pose; - lanelet::ConstLanelets extend_lanelets = data.current_lanelets; - while (rclcpp::ok()) { - const double lane_length = lanelet::utils::getLaneletLength2d(extend_lanelets); - const auto arclength = lanelet::utils::getArcCoordinates(extend_lanelets, ego_pose); - const auto next_lanelets = rh->getNextLanelets(extend_lanelets.back()); - - if (next_lanelets.empty()) { - break; - } + const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose; + const auto object_closest_index = + findNearestIndex(data.reference_path_rough.points, object_pose.position); + const auto object_closest_pose = + data.reference_path_rough.points.at(object_closest_index).point.pose; - if (lane_length - arclength.length < planner_data->parameters.forward_path_length) { - extend_lanelets.push_back(next_lanelets.front()); - } else { - break; - } + const auto rh = planner_data->route_handler; + if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &object.overhang_lanelet)) { + return 0.0; } - for (auto & o : objects) { - const auto & object_pose = o.object.kinematics.initial_pose_with_covariance.pose; - const auto object_closest_index = findNearestIndex(path_points, object_pose.position); - const auto object_closest_pose = path_points.at(object_closest_index).point.pose; - - const auto object_type = utils::getHighestProbLabel(o.object.classification); - const auto object_parameter = parameters->object_parameters.at(object_type); - - if (!isTargetObjectType(o.object, parameters)) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE; - data.other_objects.push_back(o); - continue; - } - - // if following condition are satisfied, ignored the objects as moving objects. - // 1. speed is higher than threshold. - // 2. keep that speed longer than the time threshold. - if (o.move_time > object_parameter.moving_time_threshold) { - o.reason = AvoidanceDebugFactor::MOVING_OBJECT; - data.other_objects.push_back(o); - continue; - } + double road_shoulder_distance = std::numeric_limits::max(); - // calc longitudinal distance from ego to closest target object footprint point. - fillLongitudinalAndLengthByClosestEnvelopeFootprint(data.reference_path_rough, ego_pos, o); - - // object is behind ego or too far. - if (o.longitudinal < -parameters->object_check_backward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_BEHIND_THRESHOLD; - data.other_objects.push_back(o); - continue; - } - if (o.longitudinal > parameters->object_check_max_forward_distance) { - o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD; - data.other_objects.push_back(o); - continue; - } + const bool get_left = isOnRight(object) && parameters->use_adjacent_lane; + const bool get_right = !isOnRight(object) && parameters->use_adjacent_lane; + const bool get_opposite = parameters->use_opposite_lane; - // Target object is behind the path goal -> ignore. - if (o.longitudinal > dist_to_goal) { - o.reason = AvoidanceDebugFactor::OBJECT_BEHIND_PATH_GOAL; - data.other_objects.push_back(o); - continue; - } + lanelet::BasicPoint3d p_overhang( + object.overhang_pose.position.x, object.overhang_pose.position.y, + object.overhang_pose.position.z); - if (o.longitudinal + o.length / 2 + parameters->object_check_goal_distance > dist_to_goal) { - o.reason = "TooNearToGoal"; - data.other_objects.push_back(o); - continue; - } + lanelet::ConstLineString3d target_line{}; - lanelet::ConstLanelet overhang_lanelet; - if (!rh->getClosestLaneletWithinRoute(object_closest_pose, &overhang_lanelet)) { - continue; + const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { + const auto lines = + rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); + const auto & line = isOnRight(object) ? lines.back() : lines.front(); + const auto d = boost::geometry::distance(object.envelope_poly, to2D(line.basicLineString())); + if (d < road_shoulder_distance) { + road_shoulder_distance = d; + target_line = line; } + }; - if (overhang_lanelet.id()) { - o.overhang_lanelet = overhang_lanelet; - lanelet::BasicPoint3d overhang_basic_pose( - o.overhang_pose.position.x, o.overhang_pose.position.y, o.overhang_pose.position.z); - - const bool get_left = isOnRight(o) && parameters->use_adjacent_lane; - const bool get_right = !isOnRight(o) && parameters->use_adjacent_lane; - const bool get_opposite = parameters->use_opposite_lane; - - lanelet::ConstLineString3d target_line{}; - o.to_road_shoulder_distance = std::numeric_limits::max(); - const auto update_road_to_shoulder_distance = [&](const auto & target_lanelet) { - const auto lines = - rh->getFurthestLinestring(target_lanelet, get_right, get_left, get_opposite, true); - const auto & line = isOnRight(o) ? lines.back() : lines.front(); - const auto d = boost::geometry::distance(o.envelope_poly, to2D(line.basicLineString())); - if (d < o.to_road_shoulder_distance) { - o.to_road_shoulder_distance = d; - target_line = line; - } - }; - - // current lanelet - { - update_road_to_shoulder_distance(overhang_lanelet); - - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, overhang_lanelet, o.overhang_pose.position, - overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } - - // previous lanelet - lanelet::ConstLanelets previous_lanelet{}; - if (rh->getPreviousLaneletsWithinRoute(overhang_lanelet, &previous_lanelet)) { - update_road_to_shoulder_distance(previous_lanelet.front()); - - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, previous_lanelet.front(), - o.overhang_pose.position, overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } + // current lanelet + { + update_road_to_shoulder_distance(object.overhang_lanelet); - // next lanelet - lanelet::ConstLanelet next_lanelet{}; - if (rh->getNextLaneletWithinRoute(overhang_lanelet, &next_lanelet)) { - update_road_to_shoulder_distance(next_lanelet); + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, object.overhang_lanelet, + object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } - o.to_road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( - rh, target_line, o.to_road_shoulder_distance, next_lanelet, o.overhang_pose.position, - overhang_basic_pose, parameters->use_hatched_road_markings, - parameters->use_intersection_areas); - } + // previous lanelet + lanelet::ConstLanelets previous_lanelet{}; + if (rh->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &previous_lanelet)) { + update_road_to_shoulder_distance(previous_lanelet.front()); - debug.bounds.push_back(target_line); - } + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, previous_lanelet.front(), + object.overhang_pose.position, p_overhang, parameters->use_hatched_road_markings, + parameters->use_intersection_areas); + } - // calculate avoid_margin dynamically - // NOTE: This calculation must be after calculating to_road_shoulder_distance. - const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o.distance_factor + - object_parameter.avoid_margin_lateral + 0.5 * vehicle_width; - const auto min_avoid_margin = object_parameter.safety_buffer_lateral + 0.5 * vehicle_width; - const auto soft_lateral_distance_limit = - o.to_road_shoulder_distance - parameters->soft_road_shoulder_margin - 0.5 * vehicle_width; - const auto hard_lateral_distance_limit = - o.to_road_shoulder_distance - parameters->hard_road_shoulder_margin - 0.5 * vehicle_width; - - const auto avoid_margin = [&]() -> boost::optional { - // Step1. check avoidable or not. - if (hard_lateral_distance_limit < min_avoid_margin) { - return boost::none; - } + // next lanelet + lanelet::ConstLanelet next_lanelet{}; + if (rh->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) { + update_road_to_shoulder_distance(next_lanelet); - // Step2. check if it should expand road shoulder margin. - if (soft_lateral_distance_limit < min_avoid_margin) { - return min_avoid_margin; - } + road_shoulder_distance = extendToRoadShoulderDistanceWithPolygon( + rh, target_line, road_shoulder_distance, next_lanelet, object.overhang_pose.position, + p_overhang, parameters->use_hatched_road_markings, parameters->use_intersection_areas); + } - // Step3. nominal case. avoid margin is limited by soft constraint. - return std::min(soft_lateral_distance_limit, max_avoid_margin); - }(); + return road_shoulder_distance; +} - if (!!avoid_margin) { - const auto shift_length = calcShiftLength(isOnRight(o), o.overhang_dist, avoid_margin.get()); - if (!isShiftNecessary(isOnRight(o), shift_length)) { - o.reason = "NotNeedAvoidance"; - data.other_objects.push_back(o); - continue; - } +void filterTargetObjects( + ObjectDataArray & objects, AvoidancePlanningData & data, [[maybe_unused]] DebugData & debug, + const std::shared_ptr & planner_data, + const std::shared_ptr & parameters) +{ + if (data.current_lanelets.empty()) { + return; + } - if (std::abs(shift_length) < parameters->lateral_execution_threshold) { - o.reason = "LessThanExecutionThreshold"; - data.other_objects.push_back(o); - continue; - } - } + const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now(); + const auto push_target_object = [&data, &now](auto & object) { + object.last_seen = now; + data.target_objects.push_back(object); + }; - // for non vehicle type object - if (!isVehicleTypeObject(o)) { - if (isWithinCrosswalk(o, rh->getOverallGraphPtr())) { - // avoidance module ignore pedestrian and bicycle around crosswalk - o.reason = "CrosswalkUser"; - data.other_objects.push_back(o); - } else { - // if there is no crosswalk near the object, avoidance module avoids pedestrian and bicycle - // no matter how it is shifted. - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); - } + for (auto & o : objects) { + if (!filtering_utils::isSatisfiedWithCommonCondition(o, data, planner_data, parameters)) { + data.other_objects.push_back(o); continue; } - o.is_within_intersection = isWithinIntersection(o, rh); + o.to_road_shoulder_distance = getRoadShoulderDistance(o, data, planner_data, parameters); + o.avoid_margin = filtering_utils::getAvoidMargin(o, planner_data, parameters); - // from here condition check for vehicle type objects. - if (isForceAvoidanceTarget(o, extend_lanelets, planner_data, parameters)) { - o.last_seen = now; - o.avoid_margin = avoid_margin; - data.target_objects.push_back(o); - continue; - } - - // Object is on center line -> ignore. - if (std::abs(o.lateral) < parameters->threshold_distance_object_is_on_center) { - o.reason = AvoidanceDebugFactor::TOO_NEAR_TO_CENTERLINE; + if (filtering_utils::isNoNeedAvoidanceBehavior(o, parameters)) { data.other_objects.push_back(o); continue; } - lanelet::BasicPoint2d object_centroid(o.centroid.x(), o.centroid.y()); - - /** - * Is not object in adjacent lane? - * - Yes -> Is parking object? - * - Yes -> the object is avoidance target. - * - No -> ignore this object. - * - No -> the object is avoidance target no matter whether it is parking object or not. - */ - const auto is_in_ego_lane = - within(object_centroid, overhang_lanelet.polygon2d().basicPolygon()); - if (is_in_ego_lane) { - /** - * TODO(Satoshi Ota) use intersection area - * under the assumption that there is no parking vehicle inside intersection, - * ignore all objects that is in the ego lane as not parking objects. - */ - std::string turn_direction = overhang_lanelet.attributeOr("turn_direction", "else"); - if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { - o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + if (filtering_utils::isVehicleTypeObject(o)) { + if (!filtering_utils::isSatisfiedWithVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } - - const auto centerline_pose = - lanelet::utils::getClosestCenterPose(overhang_lanelet, object_pose.position); - lanelet::BasicPoint3d centerline_point( - centerline_pose.position.x, centerline_pose.position.y, centerline_pose.position.z); - - // ============================================ <- most_left_lanelet.leftBound() - // y road shoulder - // ^ ------------------------------------------ - // | x + - // +---> --- object closest lanelet --- o ----- <- object_closest_lanelet.centerline() - // - // -------------------------------------------- - // +: object position - // o: nearest point on centerline - - bool is_left_side_parked_vehicle = false; - if (!isOnRight(o)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_left_road_lanelet = rh->getMostLeftLanelet(overhang_lanelet); - const auto most_left_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_left_road_lanelet.leftBound()); - - lanelet::ConstLanelet most_left_lanelet = most_left_road_lanelet; - const lanelet::Attribute sub_type = - most_left_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_left_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_left_lanelet = ll; - } - } - - const auto center_to_left_boundary = distance2d( - to2D(most_left_lanelet.leftBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_left_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = - toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); - o.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance; - - is_left_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; - } - - bool is_right_side_parked_vehicle = false; - if (isOnRight(o)) { - auto [object_shiftable_distance, sub_type] = [&]() { - const auto most_right_road_lanelet = rh->getMostRightLanelet(overhang_lanelet); - const auto most_right_lanelet_candidates = - rh->getLaneletMapPtr()->laneletLayer.findUsages(most_right_road_lanelet.rightBound()); - - lanelet::ConstLanelet most_right_lanelet = most_right_road_lanelet; - const lanelet::Attribute sub_type = - most_right_lanelet.attribute(lanelet::AttributeName::Subtype); - - for (const auto & ll : most_right_lanelet_candidates) { - const lanelet::Attribute sub_type = ll.attribute(lanelet::AttributeName::Subtype); - if (sub_type.value() == "road_shoulder") { - most_right_lanelet = ll; - } - } - - const auto center_to_right_boundary = distance2d( - to2D(most_right_lanelet.rightBound().basicLineString()), to2D(centerline_point)); - - return std::make_pair( - center_to_right_boundary - 0.5 * o.object.shape.dimensions.y, sub_type); - }(); - - if (sub_type.value() != "road_shoulder") { - object_shiftable_distance += parameters->object_check_min_road_shoulder_width; - } - - const auto arc_coordinates = - toArcCoordinates(to2D(overhang_lanelet.centerline().basicLineString()), object_centroid); - o.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance; - - is_right_side_parked_vehicle = o.shiftable_ratio > parameters->object_check_shiftable_ratio; - } - - if (!is_left_side_parked_vehicle && !is_right_side_parked_vehicle) { - o.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT; + } else { + if (!filtering_utils::isSatisfiedWithNonVehicleCondition(o, data, planner_data, parameters)) { data.other_objects.push_back(o); continue; } } - o.last_seen = now; - o.avoid_margin = avoid_margin; - - // set data - data.target_objects.push_back(o); + push_target_object(o); } } @@ -1577,12 +1677,12 @@ lanelet::ConstLanelets getAdjacentLane( for (const auto & lane : ego_succeeding_lanes) { const auto opt_left_lane = rh->getLeftLanelet(lane); if (!is_right_shift && opt_left_lane) { - lanes.push_back(opt_left_lane.get()); + lanes.push_back(opt_left_lane.value()); } const auto opt_right_lane = rh->getRightLanelet(lane); if (is_right_shift && opt_right_lane) { - lanes.push_back(opt_right_lane.get()); + lanes.push_back(opt_right_lane.value()); } const auto right_opposite_lanes = rh->getRightOppositeLanelets(lane); @@ -1617,10 +1717,12 @@ std::vector getSafetyCheckTargetObjects( }); }; - const auto to_predicted_objects = [&p](const auto & objects) { + const auto to_predicted_objects = [&p, ¶meters](const auto & objects) { PredictedObjects ret{}; - std::for_each(objects.begin(), objects.end(), [&p, &ret](const auto & object) { - ret.objects.push_back(object.object); + std::for_each(objects.begin(), objects.end(), [&p, &ret, ¶meters](const auto & object) { + if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) { + ret.objects.push_back(object.object); + } }); return ret; }; diff --git a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp index d5bf67f754ab7..e6dbd2679db3c 100644 --- a/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/utils/geometric_parallel_parking/geometric_parallel_parking.cpp @@ -14,15 +14,17 @@ #include "behavior_path_planner/utils/geometric_parallel_parking/geometric_parallel_parking.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/geometry.hpp" #include "tier4_autoware_utils/math/unit_conversion.hpp" #include #include +#include + #ifdef ROS_DISTRO_GALACTIC #include #include @@ -326,7 +328,7 @@ bool GeometricParallelParking::planPullOut( return false; } -boost::optional GeometricParallelParking::calcStartPose( +std::optional GeometricParallelParking::calcStartPose( const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, const double start_pose_offset, const double R_E_far, const bool is_forward, const bool left_side_parking) { @@ -341,7 +343,7 @@ boost::optional GeometricParallelParking::calcStartPose( : std::pow(R_E_far, 2) - std::pow(arc_coordinates.distance / 2 + R_E_far, 2); if (squared_distance_to_arc_connect < 0) { // may be current_pose is behind the lane - return boost::none; + return std::nullopt; } const double dx_sign = is_forward ? -1 : 1; const double dx = 2 * std::sqrt(squared_distance_to_arc_connect) * dx_sign; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp index 4c9832c374c63..9ae50def1d358 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/default_fixed_goal_planner.cpp @@ -15,7 +15,8 @@ #include "behavior_path_planner/utils/goal_planner/default_fixed_goal_planner.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -24,7 +25,8 @@ namespace behavior_path_planner { - +using Point2d = tier4_autoware_utils::Point2d; +using autoware_auto_planning_msgs::msg::PathWithLaneId; BehaviorModuleOutput DefaultFixedGoalPlanner::plan( const std::shared_ptr & planner_data) const { @@ -38,6 +40,40 @@ BehaviorModuleOutput DefaultFixedGoalPlanner::plan( return output; } +lanelet::ConstLanelets DefaultFixedGoalPlanner::extractLaneletsFromPath( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) const +{ + const auto & rh = planner_data->route_handler; + lanelet::ConstLanelets refined_path_lanelets; + for (size_t i = 0; i < refined_path.points.size(); ++i) { + const auto & path_point = refined_path.points.at(i); + int64_t lane_id = path_point.lane_ids.at(0); + lanelet::ConstLanelet lanelet = rh->getLaneletsFromId(lane_id); + bool is_unique = + std::find(refined_path_lanelets.begin(), refined_path_lanelets.end(), lanelet) == + refined_path_lanelets.end(); + if (is_unique) { + refined_path_lanelets.push_back(lanelet); + } + } + return refined_path_lanelets; +} + +bool DefaultFixedGoalPlanner::isPathValid( + const PathWithLaneId & refined_path, + const std::shared_ptr & planner_data) const +{ + const auto lanelets = extractLaneletsFromPath(refined_path, planner_data); + // std::any_of detects whether any point lies outside lanelets + bool has_points_outside_lanelet = std::any_of( + refined_path.points.begin(), refined_path.points.end(), + [&lanelets](const auto & refined_path_point) { + return !utils::isInLanelets(refined_path_point.point.pose, lanelets); + }); + return !has_points_outside_lanelet; +} + PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( const PathWithLaneId & path, const std::shared_ptr & planner_data) const { @@ -53,11 +89,20 @@ PathWithLaneId DefaultFixedGoalPlanner::modifyPathForSmoothGoalConnection( refined_goal = goal; } } - - auto refined_path = utils::refinePathForGoal( - planner_data->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal, - goal_lane_id); - + double goal_search_radius{planner_data->parameters.refine_goal_search_radius_range}; + // TODO(shen): define in the parameter + constexpr double range_reduce_by{1.0}; // set a reasonable value, 10% - 20% of the + // refine_goal_search_radius_range is recommended + bool is_valid_path{false}; + autoware_auto_planning_msgs::msg::PathWithLaneId refined_path; + while (goal_search_radius >= 0 && !is_valid_path) { + refined_path = + utils::refinePathForGoal(goal_search_radius, M_PI * 0.5, path, refined_goal, goal_lane_id); + if (isPathValid(refined_path, planner_data)) { + is_valid_path = true; + } + goal_search_radius -= range_reduce_by; + } return refined_path; } diff --git a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp index 9322350877ad1..d5252028f880b 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/freespace_pull_over.cpp @@ -15,8 +15,8 @@ #include "behavior_path_planner/utils/goal_planner/freespace_pull_over.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -44,7 +44,7 @@ FreespacePullOver::FreespacePullOver( } } -boost::optional FreespacePullOver::plan(const Pose & goal_pose) +std::optional FreespacePullOver::plan(const Pose & goal_pose) { const Pose & current_pose = planner_data_->self_odometry->pose.pose; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp index 017ecb218bea3..55d8bc6cfda3f 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/geometric_pull_over.cpp @@ -39,7 +39,7 @@ GeometricPullOver::GeometricPullOver( planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOver::plan(const Pose & goal_pose) +std::optional GeometricPullOver::plan(const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; diff --git a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp index f498ecd9ed8ec..ee8dae231f587 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/goal_searcher.cpp @@ -15,15 +15,14 @@ #include "behavior_path_planner/utils/goal_planner/goal_searcher.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include "lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "lanelet2_extension/utility/utilities.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include -#include #include diff --git a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp index ac8e458fd0d6c..9ff452b131efa 100644 --- a/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/utils/goal_planner/shift_pull_over.cpp @@ -14,9 +14,9 @@ #include "behavior_path_planner/utils/goal_planner/shift_pull_over.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" #include "behavior_path_planner/utils/goal_planner/util.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -36,7 +36,7 @@ ShiftPullOver::ShiftPullOver( left_side_parking_{parameters.parking_policy == ParkingPolicy::LEFT_SIDE} { } -boost::optional ShiftPullOver::plan(const Pose & goal_pose) +std::optional ShiftPullOver::plan(const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const double min_jerk = parameters_.minimum_lateral_jerk; @@ -99,7 +99,7 @@ PathWithLaneId ShiftPullOver::generateReferencePath( return road_lane_reference_path; } -boost::optional ShiftPullOver::generatePullOverPath( +std::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const Pose & goal_pose, const double lateral_jerk) const { diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index fb2a96f1d5cbd..66182454ecf85 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -14,13 +14,14 @@ #include "behavior_path_planner/utils/lane_change/utils.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/parameters.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -437,8 +438,8 @@ std::vector generateDrivableLanes( drivable_lanes.at(i).left_lane = current_lane; drivable_lanes.at(i).right_lane = current_lane; - const auto left_lane = route_handler.getLeftLanelet(current_lane); - const auto right_lane = route_handler.getRightLanelet(current_lane); + const auto left_lane = route_handler.getLeftLanelet(current_lane, false, false); + const auto right_lane = route_handler.getRightLanelet(current_lane, false, false); if (!left_lane && !right_lane) { continue; } @@ -751,7 +752,7 @@ CandidateOutput assignToCandidate( return candidate_output; } -boost::optional getLaneChangeTargetLane( +std::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction) { @@ -963,7 +964,7 @@ bool passParkedObject( } const auto & leading_obj = objects.at(*leading_obj_idx); - auto debug = marker_utils::createObjectDebug(leading_obj); + auto debug = utils::path_safety_checker::createObjectDebug(leading_obj); const auto leading_obj_poly = tier4_autoware_utils::toPolygon2d(leading_obj.initial_pose.pose, leading_obj.shape); if (leading_obj_poly.outer().empty()) { @@ -988,14 +989,14 @@ bool passParkedObject( // 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) { debug.second.unsafe_reason = "delay lane change"; - marker_utils::updateCollisionCheckDebugMap(object_debug, debug, false); + utils::path_safety_checker::updateCollisionCheckDebugMap(object_debug, debug, false); return true; } return false; } -boost::optional getLeadingStaticObjectIdx( +std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, const std::vector & objects, const double object_check_min_road_shoulder_width, const double object_shiftable_ratio_threshold) @@ -1010,7 +1011,7 @@ boost::optional getLeadingStaticObjectIdx( const auto & path_end = path.points.back(); double dist_lc_start_to_leading_obj = 0.0; - boost::optional leading_obj_idx = boost::none; + std::optional leading_obj_idx = std::nullopt; for (size_t obj_idx = 0; obj_idx < objects.size(); ++obj_idx) { const auto & obj = objects.at(obj_idx); const auto & obj_pose = obj.initial_pose.pose; diff --git a/planning/behavior_path_planner/src/utils/side_shift/util.cpp b/planning/behavior_path_planner/src/utils/side_shift/util.cpp index f8127abb2d9f0..af733722562f8 100644 --- a/planning/behavior_path_planner/src/utils/side_shift/util.cpp +++ b/planning/behavior_path_planner/src/utils/side_shift/util.cpp @@ -14,7 +14,7 @@ #include "behavior_path_planner/utils/side_shift/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp index 1eaea06c023f9..cbc6d6a4a0b8d 100644 --- a/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp +++ b/planning/behavior_path_planner/src/utils/start_goal_planner_common/utils.cpp @@ -19,7 +19,7 @@ namespace behavior_path_planner::utils::start_goal_planner_common using motion_utils::calcDecelDistWithJerkAndAccConstraints; -boost::optional calcFeasibleDecelDistance( +std::optional calcFeasibleDecelDistance( std::shared_ptr planner_data, const double acc_lim, const double jerk_lim, const double target_velocity) { @@ -133,8 +133,7 @@ void updatePathProperty( { // If acceleration is close to 0, the ego predicted path will be too short, so a minimum value is // necessary to ensure a reasonable path length. - // TODO(Sugahara): define them as parameter - const double min_accel_for_ego_predicted_path = 1.0; + const double min_accel_for_ego_predicted_path = ego_predicted_path_params->min_acceleration; const double acceleration = std::max(pairs_terminal_velocity_and_accel.second, min_accel_for_ego_predicted_path); diff --git a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp index 945c386668dee..32a36e1d70509 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/freespace_pull_out.cpp @@ -14,9 +14,9 @@ #include "behavior_path_planner/utils/start_planner/freespace_pull_out.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include @@ -44,7 +44,7 @@ FreespacePullOut::FreespacePullOut( } } -boost::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) +std::optional FreespacePullOut::plan(const Pose & start_pose, const Pose & end_pose) { const auto & route_handler = planner_data_->route_handler; const double backward_path_length = planner_data_->parameters.backward_path_length; diff --git a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp index 3cf42dbb5da26..fefa6b440c096 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/geometric_pull_out.cpp @@ -14,10 +14,10 @@ #include "behavior_path_planner/utils/start_planner/geometric_pull_out.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" #include @@ -37,7 +37,7 @@ GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParame planner_.setParameters(parallel_parking_parameters_); } -boost::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) +std::optional GeometricPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { PullOutPath output; diff --git a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp index a430e18c330e1..dd1d3d3c4cca4 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/shift_pull_out.cpp @@ -14,11 +14,11 @@ #include "behavior_path_planner/utils/start_planner/shift_pull_out.hpp" -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" #include "behavior_path_planner/utils/start_goal_planner_common/utils.hpp" #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" @@ -42,7 +42,7 @@ ShiftPullOut::ShiftPullOut( { } -boost::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) +std::optional ShiftPullOut::plan(const Pose & start_pose, const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; const auto & common_parameters = planner_data_->parameters; @@ -52,7 +52,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P planner_data_->parameters.backward_path_length + parameters_.max_back_distance; const auto pull_out_lanes = getPullOutLanes(planner_data_, backward_path_length); if (pull_out_lanes.empty()) { - return boost::none; + return std::nullopt; } const auto road_lanes = utils::getExtendedCurrentLanes( @@ -61,7 +61,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P // find candidate paths auto pull_out_paths = calcPullOutPaths(*route_handler, road_lanes, start_pose, goal_pose); if (pull_out_paths.empty()) { - return boost::none; + return std::nullopt; } // extract stop objects in pull out lane for collision check @@ -155,7 +155,7 @@ boost::optional ShiftPullOut::plan(const Pose & start_pose, const P return pull_out_path; } - return boost::none; + return std::nullopt; } std::vector ShiftPullOut::calcPullOutPaths( diff --git a/planning/behavior_path_planner/src/utils/start_planner/util.cpp b/planning/behavior_path_planner/src/utils/start_planner/util.cpp index ad841822aeb0d..0b26c793155f7 100644 --- a/planning/behavior_path_planner/src/utils/start_planner/util.cpp +++ b/planning/behavior_path_planner/src/utils/start_planner/util.cpp @@ -14,10 +14,10 @@ #include "behavior_path_planner/utils/start_planner/util.hpp" -#include "behavior_path_planner/utils/create_vehicle_footprint.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/create_vehicle_footprint.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include diff --git a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp index 423fcd2b05048..9235e8e8e3de6 100644 --- a/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_planner/test/test_behavior_path_planner_node_interface.cpp @@ -48,17 +48,20 @@ std::shared_ptr generateNode() const auto behavior_path_planner_dir = ament_index_cpp::get_package_share_directory("behavior_path_planner"); + std::vector module_names; + module_names.emplace_back("behavior_path_planner::AvoidanceModuleManager"); + module_names.emplace_back("behavior_path_planner::DynamicAvoidanceModuleManager"); + module_names.emplace_back("behavior_path_planner::SideShiftModuleManager"); + module_names.emplace_back("behavior_path_planner::StartPlannerModuleManager"); + module_names.emplace_back("behavior_path_planner::GoalPlannerModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::LaneChangeLeftModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeRightModuleManager"); + module_names.emplace_back("behavior_path_planner::ExternalRequestLaneChangeLeftModuleManager"); + module_names.emplace_back("behavior_path_planner::AvoidanceByLaneChangeModuleManager"); + std::vector params; - params.emplace_back("avoidance.enable_module", true); - params.emplace_back("avoidance_by_lc.enable_module", true); - params.emplace_back("dynamic_avoidance.enable_module", true); - params.emplace_back("lane_change_right.enable_module", true); - params.emplace_back("lane_change_left.enable_module", true); - params.emplace_back("external_request_lane_change_right.enable_module", true); - params.emplace_back("external_request_lane_change_left.enable_module", true); - params.emplace_back("goal_planner.enable_module", true); - params.emplace_back("start_planner.enable_module", true); - params.emplace_back("side_shift.enable_module", true); + params.emplace_back("launch_modules", module_names); node_options.parameter_overrides(params); test_utils::updateNodeOptions( diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index ad64a7ae3feb5..3268bd7ec468e 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include #include diff --git a/planning/behavior_path_planner/test/test_utils.cpp b/planning/behavior_path_planner/test/test_utils.cpp index 46e08c6697f65..fb2f46cb8b9c0 100644 --- a/planning/behavior_path_planner/test/test_utils.cpp +++ b/planning/behavior_path_planner/test/test_utils.cpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "input.hpp" #include "lanelet2_core/Attribute.h" #include "lanelet2_core/geometry/LineString.h" diff --git a/planning/behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner_common/CMakeLists.txt new file mode 100644 index 0000000000000..5567f04d5726b --- /dev/null +++ b/planning/behavior_path_planner_common/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.14) +project(behavior_path_planner_common) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(OpenCV REQUIRED) +find_package(magic_enum CONFIG REQUIRED) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/turn_signal_decider.cpp + src/interface/steering_factor_interface.cpp + src/interface/scene_module_visitor.cpp + src/utils/utils.cpp + src/utils/path_utils.cpp + src/utils/traffic_light_utils.cpp + src/utils/path_safety_checker/safety_check.cpp + src/utils/path_safety_checker/objects_filtering.cpp + src/utils/path_shifter/path_shifter.cpp + src/utils/drivable_area_expansion/static_drivable_area.cpp + src/utils/drivable_area_expansion/drivable_area_expansion.cpp + src/utils/drivable_area_expansion/map_utils.cpp + src/utils/drivable_area_expansion/footprints.cpp + src/marker_utils/utils.cpp +) + +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC + ${EIGEN3_INCLUDE_DIR} +) + +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} +) + +if(BUILD_TESTING) + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_utilities + test/test_drivable_area_expansion.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_utilities + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_safety_check + test/test_safety_check.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_safety_check + ${PROJECT_NAME} + ) + + ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_turn_signal + test/test_turn_signal.cpp + ) + + target_link_libraries(test_${PROJECT_NAME}_turn_signal + ${PROJECT_NAME} + ) +endif() + +ament_auto_package() diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp similarity index 93% rename from planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp index ca58144464731..3dcfdcc2bdcef 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/data_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ -#define BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ -#include "behavior_path_planner/parameters.hpp" -#include "behavior_path_planner/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/parameters.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -156,7 +156,7 @@ struct PlannerData std::optional prev_modified_goal{}; std::optional prev_route_id{}; std::shared_ptr route_handler{std::make_shared()}; - std::map traffic_light_id_map; + std::map traffic_light_id_map; BehaviorPathPlannerParameters parameters{}; drivable_area_expansion::DrivableAreaExpansionParameters drivable_area_expansion_parameters{}; @@ -174,7 +174,7 @@ struct PlannerData route_handler, path, turn_signal_info, current_pose, current_vel, parameters, debug_data); } - std::optional getTrafficSignal(const int id) const + std::optional getTrafficSignal(const int64_t id) const { if (traffic_light_id_map.count(id) == 0) { return std::nullopt; @@ -208,4 +208,4 @@ struct PlannerData } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__DATA_MANAGER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__DATA_MANAGER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp index 534594d93445c..a5e2c3245f3c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_interface.hpp @@ -12,20 +12,21 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" -#include -#include +#include +#include #include #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include #include +#include #include #include #include @@ -44,9 +46,7 @@ #include #include -#include #include -#include #include #include #include @@ -54,8 +54,11 @@ namespace behavior_path_planner { +using autoware_adapi_v1_msgs::msg::PlanningBehavior; using autoware_adapi_v1_msgs::msg::SteeringFactor; using autoware_auto_planning_msgs::msg::PathWithLaneId; +using objects_of_interest_marker_interface::ColorName; +using objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface; using rtc_interface::RTCInterface; using steering_factor_interface::SteeringFactorInterface; using tier4_autoware_utils::calcOffsetPose; @@ -81,11 +84,15 @@ class SceneModuleInterface public: SceneModuleInterface( const std::string & name, rclcpp::Node & node, - std::unordered_map> rtc_interface_ptr_map) + std::unordered_map> rtc_interface_ptr_map, + std::unordered_map> + objects_of_interest_marker_interface_ptr_map) : name_{name}, logger_{node.get_logger().get_child(name)}, clock_{node.get_clock()}, rtc_interface_ptr_map_(std::move(rtc_interface_ptr_map)), + objects_of_interest_marker_interface_ptr_map_( + std::move(objects_of_interest_marker_interface_ptr_map)), steering_factor_interface_ptr_( std::make_unique(&node, utils::convertToSnakeCase(name))) { @@ -94,6 +101,10 @@ class SceneModuleInterface } } + SceneModuleInterface(const SceneModuleInterface &) = delete; + SceneModuleInterface(SceneModuleInterface &&) = delete; + SceneModuleInterface & operator=(const SceneModuleInterface &) = delete; + SceneModuleInterface & operator=(SceneModuleInterface &&) = delete; virtual ~SceneModuleInterface() = default; virtual void updateModuleParams(const std::any & parameters) = 0; @@ -191,6 +202,15 @@ class SceneModuleInterface } } + void publishObjectsOfInterestMarker() + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->publishMarkerArray(); + } + } + } + void publishSteeringFactor() { if (!steering_factor_interface_ptr_) { @@ -259,41 +279,41 @@ class SceneModuleInterface std::string name() const { return name_; } - boost::optional getStopPose() const + std::optional getStopPose() const { if (!stop_pose_) { return {}; } const auto & base_link2front = planner_data_->parameters.base_link2front; - return calcOffsetPose(stop_pose_.get(), base_link2front, 0.0, 0.0); + return calcOffsetPose(stop_pose_.value(), base_link2front, 0.0, 0.0); } - boost::optional getSlowPose() const + std::optional getSlowPose() const { if (!slow_pose_) { return {}; } const auto & base_link2front = planner_data_->parameters.base_link2front; - return calcOffsetPose(slow_pose_.get(), base_link2front, 0.0, 0.0); + return calcOffsetPose(slow_pose_.value(), base_link2front, 0.0, 0.0); } - boost::optional getDeadPose() const + std::optional getDeadPose() const { if (!dead_pose_) { return {}; } const auto & base_link2front = planner_data_->parameters.base_link2front; - return calcOffsetPose(dead_pose_.get(), base_link2front, 0.0, 0.0); + return calcOffsetPose(dead_pose_.value(), base_link2front, 0.0, 0.0); } void resetWallPoses() const { - stop_pose_ = boost::none; - slow_pose_ = boost::none; - dead_pose_ = boost::none; + stop_pose_ = std::nullopt; + slow_pose_ = std::nullopt; + dead_pose_ = std::nullopt; } rclcpp::Logger getLogger() const { return logger_; } @@ -348,10 +368,6 @@ class SceneModuleInterface StopReason stop_reason_; - bool is_simultaneously_executable_as_approved_module_{false}; - - bool is_simultaneously_executable_as_candidate_module_{false}; - bool is_locked_new_module_launch_{false}; bool is_locked_output_path_{false}; @@ -415,6 +431,17 @@ class SceneModuleInterface } } + void setObjectsOfInterestData( + const geometry_msgs::msg::Pose & obj_pose, + const autoware_auto_perception_msgs::msg::Shape & obj_shape, const ColorName & color_name) + { + for (const auto & [module_name, ptr] : objects_of_interest_marker_interface_ptr_map_) { + if (ptr) { + ptr->insertObjectData(obj_pose, obj_shape, color_name); + } + } + } + /** * @brief Return SUCCESS if plan is not needed or plan is successfully finished, * FAILURE if plan has failed, RUNNING if plan is on going. @@ -509,9 +536,9 @@ class SceneModuleInterface } StopFactor stop_factor; - stop_factor.stop_pose = stop_pose_.get(); + stop_factor.stop_pose = stop_pose_.value(); stop_factor.dist_to_stop_pose = - motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.get().position); + motion_utils::calcSignedArcLength(path.points, getEgoPosition(), stop_pose_.value().position); stop_reason_.stop_factors.push_back(stop_factor); } @@ -569,13 +596,16 @@ class SceneModuleInterface std::unordered_map> rtc_interface_ptr_map_; + std::unordered_map> + objects_of_interest_marker_interface_ptr_map_; + std::unique_ptr steering_factor_interface_ptr_; - mutable boost::optional stop_pose_{boost::none}; + mutable std::optional stop_pose_{std::nullopt}; - mutable boost::optional slow_pose_{boost::none}; + mutable std::optional slow_pose_{std::nullopt}; - mutable boost::optional dead_pose_{boost::none}; + mutable std::optional dead_pose_{std::nullopt}; mutable MarkerArray info_marker_; @@ -586,4 +616,4 @@ class SceneModuleInterface } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp similarity index 73% rename from planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp index 38101416a80b3..95976bf117096 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_manager_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_manager_interface.hpp @@ -13,10 +13,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" +#include "tier4_autoware_utils/ros/parameter.hpp" #include @@ -43,38 +44,12 @@ using SceneModuleObserver = std::weak_ptr; class SceneModuleManagerInterface { public: - SceneModuleManagerInterface( - rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - const std::vector & rtc_types) - : node_(node), - clock_(*node->get_clock()), - logger_(node->get_logger().get_child(name)), - name_(name), - enable_simultaneous_execution_as_approved_module_( - config.enable_simultaneous_execution_as_approved_module), - enable_simultaneous_execution_as_candidate_module_( - config.enable_simultaneous_execution_as_candidate_module), - enable_rtc_(config.enable_rtc), - keep_last_(config.keep_last), - max_module_num_(config.max_module_size), - priority_(config.priority) - { - for (const auto & rtc_type : rtc_types) { - const auto snake_case_name = utils::convertToSnakeCase(name); - const auto rtc_interface_name = - rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; - rtc_interface_ptr_map_.emplace( - rtc_type, std::make_shared(node, rtc_interface_name, enable_rtc_)); - } - - pub_info_marker_ = node->create_publisher("~/info/" + name, 20); - pub_debug_marker_ = node->create_publisher("~/debug/" + name, 20); - pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name, 20); - pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name, 20); - } + explicit SceneModuleManagerInterface(const std::string & name) : name_{name} {} virtual ~SceneModuleManagerInterface() = default; + virtual void init(rclcpp::Node * node) = 0; + void updateIdleModuleInstance() { if (idle_module_ptr_) { @@ -134,21 +109,21 @@ class SceneModuleManagerInterface const auto opt_stop_pose = m.lock()->getStopPose(); if (!!opt_stop_pose) { const auto virtual_wall = createStopVirtualWallMarker( - opt_stop_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); + opt_stop_pose.value(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } const auto opt_slow_pose = m.lock()->getSlowPose(); if (!!opt_slow_pose) { const auto virtual_wall = createSlowDownVirtualWallMarker( - opt_slow_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); + opt_slow_pose.value(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } const auto opt_dead_pose = m.lock()->getDeadPose(); if (!!opt_dead_pose) { const auto virtual_wall = createDeadLineVirtualWallMarker( - opt_dead_pose.get(), m.lock()->name(), rclcpp::Clock().now(), marker_id); + opt_dead_pose.value(), m.lock()->name(), rclcpp::Clock().now(), marker_id); appendMarkerArray(virtual_wall, &markers); } @@ -213,7 +188,7 @@ class SceneModuleManagerInterface }); } - bool canLaunchNewModule() const { return observers_.size() < max_module_num_; } + bool canLaunchNewModule() const { return observers_.size() < config_.max_module_size; } /** * Determine if the module is always executable, regardless of the state of other modules. @@ -232,7 +207,7 @@ class SceneModuleManagerInterface return true; } - return enable_simultaneous_execution_as_approved_module_; + return config_.enable_simultaneous_execution_as_approved_module; } virtual bool isSimultaneousExecutableAsCandidateModule() const @@ -241,10 +216,10 @@ class SceneModuleManagerInterface return true; } - return enable_simultaneous_execution_as_candidate_module_; + return config_.enable_simultaneous_execution_as_candidate_module; } - bool isKeepLast() const { return keep_last_; } + bool isKeepLast() const { return config_.keep_last; } void setData(const std::shared_ptr & planner_data) { planner_data_ = planner_data; } @@ -268,7 +243,7 @@ class SceneModuleManagerInterface pub_debug_marker_->publish(MarkerArray{}); } - size_t getPriority() const { return priority_; } + size_t getPriority() const { return config_.priority; } std::string name() const { return name_; } @@ -279,13 +254,51 @@ class SceneModuleManagerInterface virtual void updateModuleParams(const std::vector & parameters) = 0; protected: - virtual std::unique_ptr createNewSceneModuleInstance() = 0; + void initInterface(rclcpp::Node * node, const std::vector & rtc_types) + { + using tier4_autoware_utils::getOrDeclareParameter; + + // init manager configuration + { + std::string ns = name_ + "."; + config_.enable_rtc = getOrDeclareParameter(*node, ns + "enable_rtc"); + config_.enable_simultaneous_execution_as_approved_module = + getOrDeclareParameter(*node, ns + "enable_simultaneous_execution_as_approved_module"); + config_.enable_simultaneous_execution_as_candidate_module = getOrDeclareParameter( + *node, ns + "enable_simultaneous_execution_as_candidate_module"); + config_.keep_last = getOrDeclareParameter(*node, ns + "keep_last"); + config_.priority = getOrDeclareParameter(*node, ns + "priority"); + config_.max_module_size = getOrDeclareParameter(*node, ns + "max_module_size"); + } - rclcpp::Node * node_; + // init rtc configuration + for (const auto & rtc_type : rtc_types) { + const auto snake_case_name = utils::convertToSnakeCase(name_); + const auto rtc_interface_name = + rtc_type == "" ? snake_case_name : snake_case_name + "_" + rtc_type; + rtc_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name, config_.enable_rtc)); + objects_of_interest_marker_interface_ptr_map_.emplace( + rtc_type, std::make_shared(node, rtc_interface_name)); + } + + // init publisher + { + pub_info_marker_ = node->create_publisher("~/info/" + name_, 20); + pub_debug_marker_ = node->create_publisher("~/debug/" + name_, 20); + pub_virtual_wall_ = node->create_publisher("~/virtual_wall/" + name_, 20); + pub_drivable_lanes_ = node->create_publisher("~/drivable_lanes/" + name_, 20); + } + + // misc + { + node_ = node; + } + } - rclcpp::Clock clock_; + virtual std::unique_ptr createNewSceneModuleInstance() = 0; - rclcpp::Logger logger_; + rclcpp::Node * node_; rclcpp::Publisher::SharedPtr pub_info_marker_; @@ -305,20 +318,12 @@ class SceneModuleManagerInterface std::unordered_map> rtc_interface_ptr_map_; - bool enable_simultaneous_execution_as_approved_module_{false}; - - bool enable_simultaneous_execution_as_candidate_module_{false}; - -private: - bool enable_rtc_; - - bool keep_last_; - - size_t max_module_num_; + std::unordered_map> + objects_of_interest_marker_interface_ptr_map_; - size_t priority_; + ModuleConfigParameters config_; }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_MANAGER_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp new file mode 100644 index 0000000000000..ffc5daa7aa2ff --- /dev/null +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/scene_module_visitor.hpp @@ -0,0 +1,47 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ + +#include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp" + +#include +namespace behavior_path_planner +{ +// Forward Declaration +class AvoidanceModule; +class AvoidanceByLCModule; +class ExternalRequestLaneChangeModule; +class LaneChangeInterface; +class StartPlannerModule; +class GoalPlannerModule; +class SideShiftModule; + +using tier4_planning_msgs::msg::AvoidanceDebugMsg; +using tier4_planning_msgs::msg::AvoidanceDebugMsgArray; + +class SceneModuleVisitor +{ +public: + void visitAvoidanceModule(const AvoidanceModule * module) const; + + std::shared_ptr getAvoidanceModuleDebugMsg() const; + +protected: + mutable std::shared_ptr avoidance_visitor_; +}; +} // namespace behavior_path_planner + +#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp similarity index 76% rename from planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp index 3a334cf67edbf..fd3ce0097c2eb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/steering_factor_interface.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/interface/steering_factor_interface.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ -#define BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ #include @@ -35,8 +35,9 @@ class SteeringFactorInterface SteeringFactorInterface(rclcpp::Node * node, const std::string & name); void publishSteeringFactor(const rclcpp::Time & stamp); void updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type, - const uint16_t direction, const uint16_t status, const std::string detail); + const std::array & poses, const std::array distances, + const std::string & behavior, const uint16_t direction, const uint16_t status, + const std::string detail); void clearSteeringFactors(); private: @@ -51,4 +52,4 @@ class SteeringFactorInterface } // namespace steering_factor_interface -#endif // BEHAVIOR_PATH_PLANNER__STEERING_FACTOR_INTERFACE_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__STEERING_FACTOR_INTERFACE_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp similarity index 92% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp index 0dee53d4f68af..95dcff8602398 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/colors.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/colors.hpp @@ -11,8 +11,8 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ #include "tier4_autoware_utils/ros/marker_helper.hpp" @@ -91,4 +91,4 @@ inline std::vector colors_list(float a = 0.99) } } // namespace marker_utils::colors -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__COLORS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__COLORS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp similarity index 87% rename from planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp index 86f1a390261c6..cfaa8de9b2fb9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/marker_utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2022 TIER IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,12 +11,12 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include #include @@ -56,11 +56,6 @@ inline int64_t bitShift(int64_t original_id) return original_id << (sizeof(int32_t) * 8 / 2); } -CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); - -void updateCollisionCheckDebugMap( - CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); - MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b); @@ -114,4 +109,4 @@ MarkerArray showFilteredObjects( const ColorRGBA & color, int32_t id); } // namespace marker_utils -#endif // BEHAVIOR_PATH_PLANNER__MARKER_UTILS__UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__MARKER_UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp similarity index 85% rename from planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 1998e7b99dd65..7832dd4e53cfc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #include #include @@ -79,17 +79,6 @@ struct BehaviorPathPlannerParameters size_t max_iteration_num{100}; double traffic_light_signal_timeout{1.0}; - ModuleConfigParameters config_avoidance; - ModuleConfigParameters config_avoidance_by_lc; - ModuleConfigParameters config_dynamic_avoidance; - ModuleConfigParameters config_start_planner; - ModuleConfigParameters config_goal_planner; - ModuleConfigParameters config_side_shift; - ModuleConfigParameters config_lane_change_left; - ModuleConfigParameters config_lane_change_right; - ModuleConfigParameters config_ext_request_lane_change_left; - ModuleConfigParameters config_ext_request_lane_change_right; - double backward_path_length; double forward_path_length; double backward_length_buffer_for_end_of_lane; @@ -146,4 +135,4 @@ struct BehaviorPathPlannerParameters bool visualize_maximum_drivable_area; }; -#endif // BEHAVIOR_PATH_PLANNER__PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp similarity index 93% rename from planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp index 3a983d66eb98c..372041c7fb586 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/turn_signal_decider.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/turn_signal_decider.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ -#define BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ -#include +#include #include #include @@ -101,7 +101,7 @@ class TurnSignalDecider std::pair getIntersectionPoseAndDistance(); private: - boost::optional getIntersectionTurnSignalInfo( + std::optional getIntersectionTurnSignalInfo( const PathWithLaneId & path, const Pose & current_pose, const double current_vel, const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold); @@ -136,4 +136,4 @@ class TurnSignalDecider }; } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__TURN_SIGNAL_DECIDER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__TURN_SIGNAL_DECIDER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp index a4f71305c3fbf..edb6d32bda205 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/create_vehicle_footprint.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/create_vehicle_footprint.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ #include #include @@ -44,4 +44,4 @@ inline tier4_autoware_utils::LinearRing2d createVehicleFootprint( return footprint; } -#endif // BEHAVIOR_PATH_PLANNER__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__CREATE_VEHICLE_FOOTPRINT_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp index 3f6f107cdce51..d0eec8f2b8901 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp @@ -12,12 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -95,4 +95,6 @@ std::vector calculate_smoothed_curvatures( } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ +// clang-format off +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__DRIVABLE_AREA_EXPANSION_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp similarity index 83% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp index 9591ac0582e04..297092d0cdd68 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -53,4 +53,4 @@ MultiPolygon2d create_object_footprints( const autoware_auto_perception_msgs::msg::PredictedObjects & objects, const DrivableAreaExpansionParameters & params); } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp similarity index 78% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp index 8c6bdb8a6943b..dc4a174c2ebf7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -41,4 +41,4 @@ SegmentRtree extract_uncrossable_segments( bool has_types(const lanelet::ConstLineString3d & ls, const std::vector & types); } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__MAP_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp index 2f9604223a248..6d1497397ad27 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ #include #include @@ -126,4 +126,4 @@ struct DrivableAreaExpansionParameters }; } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PARAMETERS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp similarity index 95% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp index 3f5327acbca68..4c1469d46e03a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include @@ -190,4 +190,4 @@ inline LineString2d sub_linestring( } } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__PATH_PROJECTION_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp similarity index 90% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index e7a05bc473c03..284b4aad20a6a 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ -#include +#include #include #include @@ -24,7 +24,7 @@ namespace behavior_path_planner::utils { using drivable_area_expansion::DrivableAreaExpansionParameters; -boost::optional getOverlappedLaneletId(const std::vector & lanes); +std::optional getOverlappedLaneletId(const std::vector & lanes); std::vector cutOverlappedLanes( PathWithLaneId & path, const std::vector & lanes); @@ -96,4 +96,4 @@ std::vector combineDrivableLanes( } // namespace behavior_path_planner::utils -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__STATIC_DRIVABLE_AREA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp similarity index 88% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp index 8da1521db6c28..eae53b027655c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/drivable_area_expansion/types.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ #include "tier4_autoware_utils/geometry/boost_geometry.hpp" @@ -57,4 +57,4 @@ struct Projection enum Side { LEFT, RIGHT }; } // namespace drivable_area_expansion -#endif // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__DRIVABLE_AREA_EXPANSION__TYPES_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp similarity index 78% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp index 67588ed0a67b6..018e6bd2a3d6e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -30,6 +30,22 @@ #include #include +namespace behavior_path_planner::utils::path_safety_checker::filter +{ + +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; + +bool velocity_filter( + const PredictedObject & object, double velocity_threshold, double max_velocity); + +bool position_filter( + PredictedObject & object, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance); + +} // namespace behavior_path_planner::utils::path_safety_checker::filter + namespace behavior_path_planner::utils::path_safety_checker { @@ -233,6 +249,42 @@ TargetObjectsOnLane createTargetObjectsOnLane( bool isTargetObjectType( const PredictedObject & object, const ObjectTypesToCheck & target_object_types); +/** + * @brief Filters objects in the 'selected' container based on the provided filter function. + * + * This function partitions the 'selected' container based on the 'filter' function + * and moves objects that satisfy the filter condition to the 'removed' container. + * + * @tparam Func The type of the filter function. + * @param selected [in,out] The container of objects to be filtered. + * @param removed [out] The container where objects not satisfying the filter condition are moved. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & selected, PredictedObjects & removed, Func filter) +{ + auto partitioned = std::partition(selected.objects.begin(), selected.objects.end(), filter); + removed.objects.insert(removed.objects.end(), partitioned, selected.objects.end()); + selected.objects.erase(partitioned, selected.objects.end()); +} + +/** + * @brief Filters objects in the 'objects' container based on the provided filter function. + * + * This function is an overload that simplifies filtering when you don't need to specify a separate + * 'removed' container. It internally creates a 'removed_objects' container and calls the main + * 'filterObjects' function. + * + * @tparam Func The type of the filter function. + * @param objects [in,out] The container of objects to be filtered. + * @param filter The filter function that determines whether an object should be removed. + */ +template +void filterObjects(PredictedObjects & objects, Func filter) +{ + [[maybe_unused]] PredictedObjects removed_objects{}; + filterObjects(objects, removed_objects, filter); +} } // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp similarity index 86% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp index 3634586c40540..d10a2f38fb975 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT #include @@ -21,6 +21,8 @@ #include #include +#include + #include #include #include @@ -134,12 +136,21 @@ struct RSSparams double rear_vehicle_deceleration{0.0}; ///< brake parameter }; +struct IntegralPredictedPolygonParams +{ + double forward_margin{0.0}; ///< Forward margin for extended ego polygon for collision check. + double backward_margin{0.0}; ///< Backward margin for extended ego polygon for collision check. + double lat_margin{0.0}; ///< Lateral margin for extended ego polygon for collision check. + double time_horizon{0.0}; ///< Time horizon for object's prediction. +}; + /** * @brief Parameters for generating the ego vehicle's predicted path. */ struct EgoPredictedPathParams { double min_velocity{0.0}; ///< Minimum velocity. + double min_acceleration{0.0}; ///< Minimum acceleration double acceleration{0.0}; ///< Acceleration value. double max_velocity{0.0}; ///< Maximum velocity. double time_horizon_for_front_object{0.0}; ///< Time horizon for front object. @@ -173,11 +184,15 @@ struct ObjectsFilteringParams struct SafetyCheckParams { bool enable_safety_check{false}; ///< Enable safety checks. + std::string method{"RSS"}; /// Method to use for safety checks. + /// possible values: ["RSS", "integral_predicted_polygon"] + double keep_unsafe_time{0.0}; ///< Time to keep unsafe before changing to safe. double hysteresis_factor_expand_rate{ 0.0}; ///< Hysteresis factor to expand/shrink polygon with the value. double backward_path_length{0.0}; ///< Length of the backward lane for path generation. double forward_path_length{0.0}; ///< Length of the forward path lane for path generation. RSSparams rss_params{}; ///< Parameters related to the RSS model. + IntegralPredictedPolygonParams integral_predicted_polygon_params{}; ///< Parameters for polygon. bool publish_debug_marker{false}; ///< Option to publish debug markers. }; @@ -200,11 +215,14 @@ struct CollisionCheckDebug std::vector obj_predicted_path; ///< object's predicted path. Polygon2d extended_ego_polygon{}; ///< Ego vehicle's extended collision polygon. Polygon2d extended_obj_polygon{}; ///< Detected object's extended collision polygon. + autoware_auto_perception_msgs::msg::Shape obj_shape; ///< object's shape. }; -using CollisionCheckDebugPair = std::pair; +using CollisionCheckDebugPair = std::pair; using CollisionCheckDebugMap = std::unordered_map; } // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ +// clang-format off +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__PATH_SAFETY_CHECKER_PARAMETERS_HPP_ // NOLINT +// clang-format on diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp similarity index 70% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp index b8bd1629d5f3f..fcb4f3f4dc207 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_safety_checker/safety_check.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include #include @@ -79,12 +79,34 @@ double calcMinimumLongitudinalLength( const double front_object_velocity, const double rear_object_velocity, const RSSparams & rss_params); -boost::optional calcInterpolatedPoseWithVelocity( +std::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time); -boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( +std::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info); +std::optional getInterpolatedPoseWithVelocityAndPolygonStamped( + const std::vector & pred_path, const double current_time, + const Shape & shape); +template +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon, const F & interpolateFunc); +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon); +ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObject & object, const double time_horizon, + const bool check_all_predicted_path); +ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObjects & objects, const double time_horizon, + const bool check_all_predicted_path); + +bool checkSafetyWithRSS( + const PathWithLaneId & planned_path, + const std::vector & ego_predicted_path, + const std::vector & objects, CollisionCheckDebugMap & debug_map, + const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, + const bool check_all_predicted_path, const double hysteresis_factor); + /** * @brief Iterate the points in the ego and target's predicted path and * perform safety check for each of the iterated points. @@ -131,7 +153,15 @@ std::vector getCollidedPolygons( bool checkPolygonsIntersects( const std::vector & polys_1, const std::vector & polys_2); - +bool checkSafetyWithIntegralPredictedPolygon( + const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, + const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, + const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map); + +// debug +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj); +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe); } // namespace behavior_path_planner::utils::path_safety_checker -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SAFETY_CHECKER__SAFETY_CHECK_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp index 91ed95b14b905..099adc21d2ad7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp @@ -12,16 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ #include #include #include -#include - #include #include #include @@ -151,7 +149,7 @@ class PathShifter double getLastShiftLength() const; - boost::optional getLastShiftLine() const; + std::optional getLastShiftLine() const; /** * @brief Calculate the theoretical lateral jerk by spline shifting for current shift_lines_. @@ -239,4 +237,4 @@ class PathShifter } // namespace behavior_path_planner -#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_SHIFTER__PATH_SHIFTER_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp similarity index 89% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp index 4b3c026ae8064..4ac6f01da65fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__PATH_UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__PATH_UTILS_HPP_ -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include -#include +#include +#include #include #include @@ -105,7 +105,7 @@ PathWithLaneId calcCenterLinePath( PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & path2); -boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path); +std::optional getFirstStopPoseFromPath(const PathWithLaneId & path); BehaviorModuleOutput getReferencePath( const lanelet::ConstLanelet & current_lane, @@ -115,4 +115,4 @@ BehaviorModuleOutput createGoalAroundPath(const std::shared_ptr +#include #include #include #include @@ -27,6 +27,7 @@ #include #include +#include #include namespace behavior_path_planner::utils::traffic_light @@ -107,6 +108,27 @@ double getDistanceToNextTrafficLight( std::optional calcDistanceToRedTrafficLight( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, const std::shared_ptr & planner_data); + +/** + * @brief Checks if the vehicle is stationary within a specified distance from a red traffic light. + * + * This function first checks if the vehicle's velocity is below a minimum threshold, indicating it + * is stopped. It then calculates the distance to the nearest red traffic light using the + * calcDistanceToRedTrafficLight function. If the vehicle is within the specified distance threshold + * from the red traffic light, the function returns true, otherwise false. + * + * @param lanelets The lanelets to search for traffic lights. + * @param path The path along which to measure the distance to the traffic light. + * @param planner_data Shared pointer to the planner data containing vehicle state information. + * @param distance_threshold The maximum allowable distance from a red traffic light to consider the + * vehicle stopped. + * @return True if the vehicle is stopped within the distance threshold from a red traffic light, + * false otherwise. + */ +bool isStoppedAtRedTrafficLightWithinDistance( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, + const double distance_threshold = std::numeric_limits::infinity()); } // namespace behavior_path_planner::utils::traffic_light -#endif // BEHAVIOR_PATH_PLANNER__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__TRAFFIC_LIGHT_UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp similarity index 96% rename from planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp rename to planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 144dad3b6feab..3a7d346849e16 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ -#define BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ +#ifndef BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ +#define BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -122,7 +122,7 @@ FrenetPoint convertToFrenetPoint( return frenet_point; } -std::vector getIds(const lanelet::ConstLanelets & lanelets); +std::vector getIds(const lanelet::ConstLanelets & lanelets); // distance (arclength) calculation @@ -204,9 +204,9 @@ double calcLongitudinalDistanceFromEgoToObjects( // drivable area generation lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes); lanelet::ConstLanelets transformToLanelets(const std::vector & drivable_lanes); -boost::optional getRightLanelet( +std::optional getRightLanelet( const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); -boost::optional getLeftLanelet( +std::optional getLeftLanelet( const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes); // goal management @@ -354,11 +354,11 @@ size_t findNearestSegmentIndex( const auto nearest_idx = motion_utils::findNearestSegmentIndex(points, pose, dist_threshold, yaw_threshold); if (nearest_idx) { - return nearest_idx.get(); + return nearest_idx.value(); } return motion_utils::findNearestSegmentIndex(points, pose.position); } } // namespace behavior_path_planner::utils -#endif // BEHAVIOR_PATH_PLANNER__UTILS__UTILS_HPP_ +#endif // BEHAVIOR_PATH_PLANNER_COMMON__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml new file mode 100644 index 0000000000000..b289ce86ca60e --- /dev/null +++ b/planning/behavior_path_planner_common/package.xml @@ -0,0 +1,76 @@ + + + + behavior_path_planner_common + 0.1.0 + The behavior_path_planner_common package + + + Zulfaqar Azmi + Mamoru Sobue + Satoshi Ota + + + Maxime CLEMENT + Takayuki Murooka + Satoshi Ota + + + Tomoya Kimura + Shumpei Wakabayashi + Takayuki Murooka + Kosuke Takeuchi + Fumiya Watanabe + Takamasa Horibe + Zulfaqar Azmi + + Apache License 2.0 + + Taiki Tanaka + Takamasa Horibe + Satoshi Ota + Fumiya Watanabe + Zulfaqar Azmi + Kosuke Takeuchi + Yutaka Shimizu + Takayuki Murooka + Ryohsuke Mitsudome + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_tf2 + autoware_perception_msgs + freespace_planning_algorithms + geometry_msgs + interpolation + lanelet2_extension + libboost-dev + libopencv-dev + magic_enum + motion_utils + object_recognition_utils + objects_of_interest_marker_interface + rclcpp + route_handler + rtc_interface + sensor_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_planning_msgs + vehicle_info_util + visualization_msgs + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp b/planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp similarity index 74% rename from planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp rename to planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp index 42c8ae15919b1..af1f9c4c43f70 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_visitor.cpp +++ b/planning/behavior_path_planner_common/src/interface/scene_module_visitor.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/scene_module/scene_module_visitor.hpp" +#include "behavior_path_planner_common/interface/scene_module_visitor.hpp" -#include "behavior_path_planner/scene_module/scene_module_interface.hpp" +#include "behavior_path_planner_common/interface/scene_module_interface.hpp" namespace behavior_path_planner { @@ -22,9 +22,4 @@ std::shared_ptr SceneModuleVisitor::getAvoidanceModuleDe { return avoidance_visitor_; } - -std::shared_ptr SceneModuleVisitor::getLaneChangeModuleDebugMsg() const -{ - return lane_change_visitor_; -} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/steering_factor_interface.cpp b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp similarity index 89% rename from planning/behavior_path_planner/src/steering_factor_interface.cpp rename to planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp index 9ba79f1266fdc..93ae7e8289902 100644 --- a/planning/behavior_path_planner/src/steering_factor_interface.cpp +++ b/planning/behavior_path_planner_common/src/interface/steering_factor_interface.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/steering_factor_interface.hpp" +#include "behavior_path_planner_common/interface/steering_factor_interface.hpp" namespace steering_factor_interface { @@ -32,8 +32,9 @@ void SteeringFactorInterface::publishSteeringFactor(const rclcpp::Time & stamp) } void SteeringFactorInterface::updateSteeringFactor( - const std::array & pose, const std::array distance, const uint16_t type, - const uint16_t direction, const uint16_t status, const std::string detail) + const std::array & pose, const std::array distance, + const std::string & behavior, const uint16_t direction, const uint16_t status, + const std::string detail) { std::lock_guard lock(mutex_); SteeringFactor factor; @@ -41,7 +42,7 @@ void SteeringFactorInterface::updateSteeringFactor( std::array converted_distance; for (int i = 0; i < 2; ++i) converted_distance[i] = static_cast(distance[i]); factor.distance = converted_distance; - factor.type = type; + factor.behavior = behavior; factor.direction = direction; factor.status = status; factor.detail = detail; diff --git a/planning/behavior_path_planner/src/marker_utils/utils.cpp b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp similarity index 96% rename from planning/behavior_path_planner/src/marker_utils/utils.cpp rename to planning/behavior_path_planner_common/src/marker_utils/utils.cpp index f1f681fed06a0..4dc422a81d22c 100644 --- a/planning/behavior_path_planner/src/marker_utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/marker_utils/utils.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/utils.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" -#include "behavior_path_planner/marker_utils/colors.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/marker_utils/colors.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -42,28 +42,6 @@ using tier4_autoware_utils::createMarkerScale; using tier4_autoware_utils::createPoint; using visualization_msgs::msg::Marker; -CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) -{ - CollisionCheckDebug debug; - debug.current_obj_pose = obj.initial_pose.pose; - debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); - debug.current_twist = obj.initial_twist.twist; - return {tier4_autoware_utils::toHexString(obj.uuid), debug}; -} - -void updateCollisionCheckDebugMap( - CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe) -{ - auto & [key, element] = object_debug; - element.is_safe = is_safe; - if (debug_map.find(key) != debug_map.end()) { - debug_map[key] = element; - return; - } - - debug_map.insert(object_debug); -} - MarkerArray createPoseMarkerArray( const Pose & pose, std::string && ns, const int32_t & id, const float & r, const float & g, const float & b) diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp similarity index 99% rename from planning/behavior_path_planner/src/turn_signal_decider.cpp rename to planning/behavior_path_planner_common/src/turn_signal_decider.cpp index d1b8bdc806bf4..933330dafae7e 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner_common/src/turn_signal_decider.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/turn_signal_decider.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -124,7 +124,7 @@ std::pair TurnSignalDecider::getIntersectionPoseAndDistance() return std::make_pair(intersection_pose_point_, intersection_distance_); } -boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo( +std::optional TurnSignalDecider::getIntersectionTurnSignalInfo( const PathWithLaneId & path, const Pose & current_pose, const double current_vel, const size_t current_seg_idx, const RouteHandler & route_handler, const double nearest_dist_threshold, const double nearest_yaw_threshold) diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp similarity index 96% rename from planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp rename to planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp index ebc1f686865a6..4fe9a3cd6e10f 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/drivable_area_expansion.cpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp similarity index 93% rename from planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp rename to planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp index 2e6d4929fdf02..6b323be6c6328 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/footprints.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/drivable_area_expansion/footprints.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/footprints.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/parameters.hpp" #include #include diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp similarity index 95% rename from planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp rename to planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp index 6ed14138c62e4..2eb55599b83f9 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/map_utils.cpp @@ -1,4 +1,3 @@ - // Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); @@ -13,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/map_utils.hpp" #include diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/drivable_area_expansion/static_drivable_area.cpp rename to planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index becbf4ceb508b..f6344b94a6bc5 100644 --- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -11,9 +11,9 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include @@ -88,7 +88,7 @@ geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( const auto offset_point = motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_point ? offset_point.get() : points.at(nearest_segment_idx); + return offset_point ? offset_point.value() : points.at(nearest_segment_idx); } geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( @@ -100,13 +100,13 @@ geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( const auto offset_point = motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_point ? offset_point.get() : points.at(nearest_segment_idx + 1); + return offset_point ? offset_point.value() : points.at(nearest_segment_idx + 1); } } // namespace namespace behavior_path_planner::utils::drivable_area_processing { -boost::optional> intersectBound( +std::optional> intersectBound( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const std::vector & bound, const size_t seg_idx1, const size_t seg_idx2) @@ -125,7 +125,7 @@ boost::optional> intersectBound( return result; } } - return boost::none; + return std::nullopt; } double calcDistanceFromPointToSegment( @@ -444,7 +444,7 @@ std::vector getPolygonPointsInsideBounds( const auto start_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( bound, start_point.bound_seg_idx, start_point.lon_dist_to_segment); if (start_point_on_bound) { - start_point.point = start_point_on_bound.get(); + start_point.point = start_point_on_bound.value(); valid_inside_polygon.insert(valid_inside_polygon.begin(), start_point); } } @@ -453,7 +453,7 @@ std::vector getPolygonPointsInsideBounds( const auto end_point_on_bound = motion_utils::calcLongitudinalOffsetPoint( bound, end_point.bound_seg_idx, end_point.lon_dist_to_segment); if (end_point_on_bound) { - end_point.point = end_point_on_bound.get(); + end_point.point = end_point_on_bound.value(); valid_inside_polygon.insert(valid_inside_polygon.end(), end_point); } } @@ -515,7 +515,7 @@ namespace behavior_path_planner::utils { using tier4_autoware_utils::Point2d; -boost::optional getOverlappedLaneletId(const std::vector & lanes) +std::optional getOverlappedLaneletId(const std::vector & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { const auto lanelets = utils::transformToLanelets(lanes); @@ -1447,7 +1447,7 @@ void makeBoundLongitudinallyMonotonic( const auto get_intersect_idx = []( const auto & bound_with_pose, const auto start_idx, - const auto & p1, const auto & p2) -> boost::optional { + const auto & p1, const auto & p2) -> std::optional { std::vector> intersects; for (size_t i = start_idx; i < bound_with_pose.size() - 1; i++) { const auto opt_intersect = @@ -1461,7 +1461,7 @@ void makeBoundLongitudinallyMonotonic( } if (intersects.empty()) { - return boost::none; + return std::nullopt; } std::sort(intersects.begin(), intersects.end(), [&](const auto & a, const auto & b) { @@ -1492,7 +1492,7 @@ void makeBoundLongitudinallyMonotonic( } if (i + 1 == path_points.size()) { - for (size_t j = intersect_idx.get(); j < bound_with_pose.size(); j++) { + for (size_t j = intersect_idx.value(); j < bound_with_pose.size(); j++) { if (j + 1 == bound_with_pose.size()) { const auto yaw = calcAzimuthAngle(bound_with_pose.at(j - 1).position, bound_with_pose.at(j).position); @@ -1504,14 +1504,14 @@ void makeBoundLongitudinallyMonotonic( } } } else { - for (size_t j = intersect_idx.get() + 1; j < bound_with_pose.size(); j++) { + for (size_t j = intersect_idx.value() + 1; j < bound_with_pose.size(); j++) { set_orientation(ret, j, getPose(path_points.at(i)).orientation); } } constexpr size_t OVERLAP_CHECK_NUM = 3; start_bound_idx = - intersect_idx.get() < OVERLAP_CHECK_NUM ? 0 : intersect_idx.get() - OVERLAP_CHECK_NUM; + intersect_idx.value() < OVERLAP_CHECK_NUM ? 0 : intersect_idx.value() - OVERLAP_CHECK_NUM; } return ret; diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp similarity index 84% rename from planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp rename to planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp index f6644a4f91e8f..84c02b3b325de 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/objects_filtering.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/path_safety_checker/objects_filtering.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" #include @@ -22,9 +22,32 @@ #include -namespace behavior_path_planner::utils::path_safety_checker +#include + +namespace behavior_path_planner::utils::path_safety_checker::filter +{ +bool velocity_filter(const PredictedObject & object, double velocity_threshold, double max_velocity) { + const auto v_norm = std::hypot( + object.kinematics.initial_twist_with_covariance.twist.linear.x, + object.kinematics.initial_twist_with_covariance.twist.linear.y); + return (velocity_threshold < v_norm && v_norm < max_velocity); +} +bool position_filter( + const PredictedObject & object, const std::vector & path_points, + const geometry_msgs::msg::Point & current_pose, const double forward_distance, + const double backward_distance) +{ + const auto dist_ego_to_obj = motion_utils::calcSignedArcLength( + path_points, current_pose, object.kinematics.initial_pose_with_covariance.pose.position); + + return (backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance); +} +} // namespace behavior_path_planner::utils::path_safety_checker::filter + +namespace behavior_path_planner::utils::path_safety_checker +{ bool isCentroidWithinLanelet(const PredictedObject & object, const lanelet::ConstLanelet & lanelet) { const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; @@ -55,9 +78,8 @@ PredictedObjects filterObjects( const double object_check_backward_distance = params->object_check_backward_distance; const ObjectTypesToCheck & target_object_types = params->object_types_to_check; - PredictedObjects filtered_objects; - - filtered_objects = filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); + PredictedObjects filtered_objects = + filterObjectsByVelocity(*objects, ignore_object_velocity_threshold, false); filterObjectsByClass(filtered_objects, target_object_types); @@ -77,24 +99,19 @@ PredictedObjects filterObjectsByVelocity( { if (remove_above_threshold) { return filterObjectsByVelocity(objects, -velocity_threshold, velocity_threshold); - } else { - return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); } + return filterObjectsByVelocity(objects, velocity_threshold, std::numeric_limits::max()); } PredictedObjects filterObjectsByVelocity( const PredictedObjects & objects, double velocity_threshold, double max_velocity) { - PredictedObjects filtered; - filtered.header = objects.header; - for (const auto & obj : objects.objects) { - const auto v_norm = std::hypot( - obj.kinematics.initial_twist_with_covariance.twist.linear.x, - obj.kinematics.initial_twist_with_covariance.twist.linear.y); - if (velocity_threshold < v_norm && v_norm < max_velocity) { - filtered.objects.push_back(obj); - } - } + const auto filter = [&](const auto & object) { + return filter::velocity_filter(object, velocity_threshold, max_velocity); + }; + + auto filtered = objects; + filterObjects(filtered, filter); return filtered; } @@ -103,43 +120,22 @@ void filterObjectsByPosition( const geometry_msgs::msg::Point & current_pose, const double forward_distance, const double backward_distance) { - // Create a new container to hold the filtered objects - PredictedObjects filtered; - filtered.header = objects.header; - - // Reserve space in the vector to avoid reallocations - filtered.objects.reserve(objects.objects.size()); - - for (const auto & obj : objects.objects) { - const double dist_ego_to_obj = motion_utils::calcSignedArcLength( - path_points, current_pose, obj.kinematics.initial_pose_with_covariance.pose.position); - - if (-backward_distance < dist_ego_to_obj && dist_ego_to_obj < forward_distance) { - filtered.objects.push_back(obj); - } - } + const auto filter = [&](const auto & object) { + return filter::position_filter( + object, path_points, current_pose, forward_distance, -backward_distance); + }; - // Replace the original objects with the filtered list - objects.objects = std::move(filtered.objects); - return; + filterObjects(objects, filter); } void filterObjectsByClass( PredictedObjects & objects, const ObjectTypesToCheck & target_object_types) { - PredictedObjects filtered_objects; - - for (auto & object : objects.objects) { - const auto is_object_type = isTargetObjectType(object, target_object_types); - - // If the object type matches any of the target types, add it to the filtered list - if (is_object_type) { - filtered_objects.objects.push_back(object); - } - } + const auto filter = [&](const auto & object) { + return isTargetObjectType(object, target_object_types); + }; - // Replace the original objects with the filtered list - objects = std::move(filtered_objects); + filterObjects(objects, filter); } std::pair, std::vector> separateObjectIndicesByLanelets( @@ -154,16 +150,11 @@ std::pair, std::vector> separateObjectIndicesByLanel std::vector other_indices; for (size_t i = 0; i < objects.objects.size(); i++) { - bool is_filtered_object = false; - for (const auto & llt : target_lanelets) { - if (condition(objects.objects.at(i), llt)) { - target_indices.push_back(i); - is_filtered_object = true; - break; - } - } - - if (!is_filtered_object) { + const auto filter = [&](const auto & llt) { return condition(objects.objects.at(i), llt); }; + const auto found = std::find_if(target_lanelets.begin(), target_lanelets.end(), filter); + if (found != target_lanelets.end()) { + target_indices.push_back(i); + } else { other_indices.push_back(i); } } @@ -266,13 +257,11 @@ bool isCentroidWithinLanelets( const auto & object_pos = object.kinematics.initial_pose_with_covariance.pose.position; lanelet::BasicPoint2d object_centroid(object_pos.x, object_pos.y); - for (const auto & llt : target_lanelets) { - if (boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon())) { - return true; - } - } + const auto is_within = [&](const auto & llt) { + return boost::geometry::within(object_centroid, llt.polygon2d().basicPolygon()); + }; - return false; + return std::any_of(target_lanelets.begin(), target_lanelets.end(), is_within); } ExtendedPredictedObject transform( diff --git a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp similarity index 64% rename from planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp rename to planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp index 40aa184105edb..f5d76f5cd4d35 100644 --- a/planning/behavior_path_planner/src/utils/path_safety_checker/safety_check.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_safety_checker/safety_check.cpp @@ -12,14 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "interpolation/linear_interpolation.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "tier4_autoware_utils/geometry/boost_polygon_utils.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" +#include #include #include +#include #include namespace behavior_path_planner::utils::path_safety_checker @@ -180,6 +184,27 @@ Polygon2d createExtendedPolygon( : tier4_autoware_utils::inverseClockwise(polygon); } +std::vector createExtendedPolygonsFromPoseWithVelocityStamped( + const std::vector & predicted_path, const VehicleInfo & vehicle_info, + const double forward_margin, const double backward_margin, const double lat_margin) +{ + std::vector polygons{}; + polygons.reserve(predicted_path.size()); + + for (const auto & elem : predicted_path) { + const auto & pose = elem.pose; + const double base_to_front = vehicle_info.max_longitudinal_offset_m + forward_margin; + const double base_to_rear = vehicle_info.rear_overhang_m + backward_margin; + const double width = vehicle_info.vehicle_width_m + lat_margin * 2; + + const auto polygon = + tier4_autoware_utils::toFootprint(pose, base_to_front, base_to_rear, width); + polygons.push_back(polygon); + } + + return polygons; +} + PredictedPath convertToPredictedPath( const std::vector & path, const double time_resolution) { @@ -223,12 +248,12 @@ double calcMinimumLongitudinalLength( return rss_params.longitudinal_velocity_delta_time * std::abs(max_vel) + lon_threshold; } -boost::optional calcInterpolatedPoseWithVelocity( +std::optional calcInterpolatedPoseWithVelocity( const std::vector & path, const double relative_time) { // Check if relative time is in the valid range if (path.empty() || relative_time < 0.0) { - return boost::none; + return std::nullopt; } constexpr double epsilon = 1e-6; @@ -247,10 +272,10 @@ boost::optional calcInterpolatedPoseWithVelocity( } } - return boost::none; + return std::nullopt; } -boost::optional getInterpolatedPoseWithVelocityAndPolygonStamped( +std::optional getInterpolatedPoseWithVelocityAndPolygonStamped( const std::vector & pred_path, const double current_time, const VehicleInfo & ego_info) { @@ -273,6 +298,177 @@ boost::optional getInterpolatedPoseWithVeloci return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, ego_polygon}; } +std::optional getInterpolatedPoseWithVelocityAndPolygonStamped( + const std::vector & pred_path, const double current_time, + const Shape & shape) +{ + auto toPoseWithVelocityStampedVector = [](const auto & pred_path) { + std::vector path; + path.reserve(pred_path.size()); + for (const auto & elem : pred_path) { + path.push_back(PoseWithVelocityStamped{elem.time, elem.pose, elem.velocity}); + } + return path; + }; + + const auto interpolation_result = + calcInterpolatedPoseWithVelocity(toPoseWithVelocityStampedVector(pred_path), current_time); + + if (!interpolation_result) { + return {}; + } + + const auto & pose = interpolation_result->pose; + const auto & velocity = interpolation_result->velocity; + + const auto obj_polygon = tier4_autoware_utils::toPolygon2d(pose, shape); + + return PoseWithVelocityAndPolygonStamped{current_time, pose, velocity, obj_polygon}; +} + +template +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon, const F & interpolateFunc) +{ + std::vector filtered_path; + + for (const auto & elem : path) { + if (elem.time < time_horizon) { + filtered_path.push_back(elem); + } else { + break; + } + } + + const auto interpolated_opt = interpolateFunc(path, time_horizon); + if (interpolated_opt) { + filtered_path.push_back(*interpolated_opt); + } + + return filtered_path; +}; + +std::vector filterPredictedPathByTimeHorizon( + const std::vector & path, const double time_horizon) +{ + return filterPredictedPathByTimeHorizon( + path, time_horizon, [](const auto & path, const auto & time) { + return calcInterpolatedPoseWithVelocity(path, time); + }); +} + +ExtendedPredictedObject filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObject & object, const double time_horizon, + const bool check_all_predicted_path) +{ + auto filtered_object = object; + auto filtered_predicted_paths = getPredictedPathFromObj(object, check_all_predicted_path); + + for (auto & predicted_path : filtered_predicted_paths) { + // path is vector of polygon + const auto filtered_path = filterPredictedPathByTimeHorizon( + predicted_path.path, time_horizon, [&object](const auto & poses, double t) { + return getInterpolatedPoseWithVelocityAndPolygonStamped(poses, t, object.shape); + }); + predicted_path.path = filtered_path; + } + + filtered_object.predicted_paths = filtered_predicted_paths; + return filtered_object; +} + +ExtendedPredictedObjects filterObjectPredictedPathByTimeHorizon( + const ExtendedPredictedObjects & objects, const double time_horizon, + const bool check_all_predicted_path) +{ + ExtendedPredictedObjects filtered_objects; + filtered_objects.reserve(objects.size()); + + for (const auto & object : objects) { + filtered_objects.push_back( + filterObjectPredictedPathByTimeHorizon(object, time_horizon, check_all_predicted_path)); + } + + return filtered_objects; +} + +bool checkSafetyWithRSS( + const PathWithLaneId & planned_path, + const std::vector & ego_predicted_path, + const std::vector & objects, CollisionCheckDebugMap & debug_map, + const BehaviorPathPlannerParameters & parameters, const RSSparams & rss_params, + const bool check_all_predicted_path, const double hysteresis_factor) +{ + // Check for collisions with each predicted path of the object + const bool is_safe = !std::any_of(objects.begin(), objects.end(), [&](const auto & object) { + auto current_debug_data = utils::path_safety_checker::createObjectDebug(object); + + const auto obj_predicted_paths = + utils::path_safety_checker::getPredictedPathFromObj(object, check_all_predicted_path); + + return std::any_of( + obj_predicted_paths.begin(), obj_predicted_paths.end(), [&](const auto & obj_path) { + const bool has_collision = !utils::path_safety_checker::checkCollision( + planned_path, ego_predicted_path, object, obj_path, parameters, rss_params, + hysteresis_factor, current_debug_data.second); + + utils::path_safety_checker::updateCollisionCheckDebugMap( + debug_map, current_debug_data, !has_collision); + + return has_collision; + }); + }); + + return is_safe; +} + +bool checkSafetyWithIntegralPredictedPolygon( + const std::vector & ego_predicted_path, const VehicleInfo & vehicle_info, + const ExtendedPredictedObjects & objects, const bool check_all_predicted_path, + const IntegralPredictedPolygonParams & params, CollisionCheckDebugMap & debug_map) +{ + const std::vector filtered_ego_path = filterPredictedPathByTimeHorizon( + ego_predicted_path, params.time_horizon); // path is vector of pose + const std::vector extended_ego_polygons = + createExtendedPolygonsFromPoseWithVelocityStamped( + filtered_ego_path, vehicle_info, params.forward_margin, params.backward_margin, + params.lat_margin); + + const ExtendedPredictedObjects filtered_path_objects = filterObjectPredictedPathByTimeHorizon( + objects, params.time_horizon, check_all_predicted_path); // path is vector of polygon + + Polygon2d ego_integral_polygon{}; + for (const auto & ego_polygon : extended_ego_polygons) { + std::vector unions{}; + boost::geometry::union_(ego_integral_polygon, ego_polygon, unions); + if (!unions.empty()) { + ego_integral_polygon = unions.front(); + boost::geometry::correct(ego_integral_polygon); + } + } + + // check collision + for (const auto & object : filtered_path_objects) { + CollisionCheckDebugPair debug_pair = createObjectDebug(object); + for (const auto & path : object.predicted_paths) { + for (const auto & pose_with_poly : path.path) { + if (boost::geometry::overlaps(ego_integral_polygon, pose_with_poly.poly)) { + { + debug_pair.second.ego_predicted_path = ego_predicted_path; // raw path + debug_pair.second.obj_predicted_path = path.path; // raw path + debug_pair.second.extended_obj_polygon = pose_with_poly.poly; + debug_pair.second.extended_ego_polygon = + ego_integral_polygon; // time filtered extended polygon + updateCollisionCheckDebugMap(debug_map, debug_pair, false); + } + return false; + } + } + } + } + return true; +} + bool checkCollision( const PathWithLaneId & planned_path, const std::vector & predicted_ego_path, @@ -318,6 +514,8 @@ std::vector getCollidedPolygons( const auto interpolated_data = getInterpolatedPoseWithVelocityAndPolygonStamped( predicted_ego_path, current_time, ego_vehicle_info); if (!interpolated_data) { + debug.expected_obj_pose = obj_pose; + debug.extended_obj_polygon = obj_polygon; continue; } const auto & ego_pose = interpolated_data->pose; @@ -392,4 +590,28 @@ bool checkPolygonsIntersects( } return false; } + +CollisionCheckDebugPair createObjectDebug(const ExtendedPredictedObject & obj) +{ + CollisionCheckDebug debug; + debug.current_obj_pose = obj.initial_pose.pose; + debug.extended_obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape); + debug.obj_shape = obj.shape; + debug.current_twist = obj.initial_twist.twist; + return {tier4_autoware_utils::toBoostUUID(obj.uuid), debug}; +} + +void updateCollisionCheckDebugMap( + CollisionCheckDebugMap & debug_map, CollisionCheckDebugPair & object_debug, bool is_safe) +{ + auto & [key, element] = object_debug; + element.is_safe = is_safe; + if (debug_map.find(key) != debug_map.end()) { + debug_map[key] = element; + return; + } + + debug_map.insert(object_debug); +} + } // namespace behavior_path_planner::utils::path_safety_checker diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp similarity index 98% rename from planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp rename to planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp index df2d82b1072d4..7a74ca598de8f 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_shifter/path_shifter.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" #include #include @@ -630,7 +630,7 @@ double PathShifter::getLastShiftLength() const return furthest->end_shift_length; } -boost::optional PathShifter::getLastShiftLine() const +std::optional PathShifter::getLastShiftLine() const { if (shift_lines_.empty()) { return {}; diff --git a/planning/behavior_path_planner/src/utils/path_utils.cpp b/planning/behavior_path_planner_common/src/utils/path_utils.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/path_utils.cpp rename to planning/behavior_path_planner_common/src/utils/path_utils.cpp index 44501977eacd6..994a7ba1a19d5 100644 --- a/planning/behavior_path_planner/src/utils/path_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/path_utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/path_utils.hpp" +#include "behavior_path_planner_common/utils/path_utils.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/static_drivable_area.hpp" -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" #include #include @@ -28,6 +28,7 @@ #include #include +#include #include #include @@ -71,13 +72,14 @@ PathWithLaneId resamplePathWithSpline( transformed_path.at(i) = path.points.at(i).point; } - const auto find_almost_same_values = [&](const std::vector & vec, double x) { + const auto find_almost_same_values = + [&](const std::vector & vec, double x) -> std::optional> { constexpr double epsilon = 0.2; const auto is_close = [&](double v, double x) { return std::abs(v - x) < epsilon; }; std::vector indices; if (vec.empty()) { - return boost::optional>(); + return std::nullopt; } for (size_t i = 0; i < vec.size(); ++i) { @@ -86,8 +88,11 @@ PathWithLaneId resamplePathWithSpline( } } - return indices.empty() ? boost::optional>() - : boost::optional>(indices); + if (indices.empty()) { + return std::nullopt; + } + + return indices; }; // Get lane ids that are not duplicated @@ -582,14 +587,14 @@ PathWithLaneId combinePath(const PathWithLaneId & path1, const PathWithLaneId & return filtered_path; } -boost::optional getFirstStopPoseFromPath(const PathWithLaneId & path) +std::optional getFirstStopPoseFromPath(const PathWithLaneId & path) { for (const auto & p : path.points) { if (std::abs(p.point.longitudinal_velocity_mps) < 0.01) { return p.point.pose; } } - return boost::none; + return std::nullopt; } BehaviorModuleOutput getReferencePath( diff --git a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp similarity index 86% rename from planning/behavior_path_planner/src/utils/traffic_light_utils.cpp rename to planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp index 9ec8531818d83..850919f539e59 100644 --- a/planning/behavior_path_planner/src/utils/traffic_light_utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/traffic_light_utils.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include #include namespace behavior_path_planner::utils::traffic_light @@ -85,7 +85,7 @@ double getDistanceToNextTrafficLight( lanelet::utils::to2D(lanelet_point).basicPoint()); for (const auto & element : current_lanelet.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().value(); const auto to_stop_line = lanelet::geometry::toArcCoordinates( lanelet::utils::to2D(current_lanelet.centerline()), @@ -112,7 +112,7 @@ double getDistanceToNextTrafficLight( } for (const auto & element : llt.regulatoryElementsAs()) { - lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().get(); + lanelet::ConstLineString3d lanelet_stop_lines = element->stopLine().value(); const auto to_stop_line = lanelet::geometry::toArcCoordinates( lanelet::utils::to2D(llt.centerline()), @@ -154,4 +154,27 @@ std::optional calcDistanceToRedTrafficLight( return std::nullopt; } + +bool isStoppedAtRedTrafficLightWithinDistance( + const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path, + const std::shared_ptr & planner_data, const double distance_threshold) +{ + const auto ego_velocity = std::hypot( + planner_data->self_odometry->twist.twist.linear.x, + planner_data->self_odometry->twist.twist.linear.y); + constexpr double minimum_speed = 0.1; + if (ego_velocity > minimum_speed) { + return false; + } + + const auto distance_to_red_traffic_light = + calcDistanceToRedTrafficLight(lanelets, path, planner_data); + + if (!distance_to_red_traffic_light) { + return false; + } + + return (distance_to_red_traffic_light < distance_threshold); +} + } // namespace behavior_path_planner::utils::traffic_light diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp similarity index 97% rename from planning/behavior_path_planner/src/utils/utils.cpp rename to planning/behavior_path_planner_common/src/utils/utils.cpp index 763719f11ebd9..88c7532b0ad72 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1,4 +1,4 @@ -// Copyright 2021-2023 Tier IV, Inc. +// Copyright 2023 TIER IV, Inc. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/utils/utils.hpp" +#include "behavior_path_planner_common/utils/utils.hpp" -#include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" +#include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" #include "motion_utils/trajectory/path_with_lane_id.hpp" #include "object_recognition_utils/predicted_path_utils.hpp" @@ -271,13 +271,13 @@ bool exists(std::vector vec, T element) return std::find(vec.begin(), vec.end(), element) != vec.end(); } -boost::optional findIndexOutOfGoalSearchRange( +std::optional findIndexOutOfGoalSearchRange( const std::vector & points, const Pose & goal, const int64_t goal_lane_id, const double max_dist = std::numeric_limits::max()) { if (points.empty()) { - return boost::none; + return std::nullopt; } // find goal index @@ -299,7 +299,7 @@ boost::optional findIndexOutOfGoalSearchRange( } } if (!found) { - return boost::none; + return std::nullopt; } } @@ -356,7 +356,7 @@ bool setGoal( if (!min_dist_out_of_circle_index_opt) { return false; } - const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.get(); + const size_t min_dist_out_of_circle_index = min_dist_out_of_circle_index_opt.value(); // create output points output_ptr->points.reserve(output_ptr->points.size() + min_dist_out_of_circle_index + 3); @@ -627,7 +627,7 @@ lanelet::ConstLanelets transformToLanelets(const std::vector & dr return lanes; } -boost::optional getRightLanelet( +std::optional getRightLanelet( const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) { for (const auto & shoulder_lane : shoulder_lanes) { @@ -639,7 +639,7 @@ boost::optional getRightLanelet( return {}; } -boost::optional getLeftLanelet( +std::optional getLeftLanelet( const lanelet::ConstLanelet & current_lane, const lanelet::ConstLanelets & shoulder_lanes) { for (const auto & shoulder_lane : shoulder_lanes) { @@ -778,9 +778,9 @@ double getSignedDistance( return arc_goal.length - arc_current.length; } -std::vector getIds(const lanelet::ConstLanelets & lanelets) +std::vector getIds(const lanelet::ConstLanelets & lanelets) { - std::vector ids; + std::vector ids; ids.reserve(lanelets.size()); for (const auto & llt : lanelets) { ids.push_back(llt.id()); @@ -881,12 +881,12 @@ std::optional getSignedDistanceFromBoundary( // Find the closest bound segment that contains the corner point in the X-direction // and calculate the lateral distance from that segment. const auto calcLateralDistanceFromBound = - [&](const Point & vehicle_corner_point) -> boost::optional> { + [&](const Point & vehicle_corner_point) -> std::optional> { Pose vehicle_corner_pose{}; vehicle_corner_pose.position = vehicle_corner_point; vehicle_corner_pose.orientation = vehicle_pose.orientation; - boost::optional> lateral_distance_with_idx{}; + std::optional> lateral_distance_with_idx{}; // Euclidean distance to find the closest segment containing the corner point. double min_distance = std::numeric_limits::max(); @@ -920,13 +920,13 @@ std::optional getSignedDistanceFromBoundary( if (lateral_distance_with_idx) { return lateral_distance_with_idx; } - return boost::optional>{}; + return std::nullopt; }; // Calculate the lateral distance for both the rear and front corners of the vehicle. - const boost::optional> rear_lateral_distance_with_idx = + const std::optional> rear_lateral_distance_with_idx = calcLateralDistanceFromBound(rear_corner_point); - const boost::optional> front_lateral_distance_with_idx = + const std::optional> front_lateral_distance_with_idx = calcLateralDistanceFromBound(front_corner_point); // If no closest bound segment was found for both corners, return an empty optional. @@ -935,23 +935,23 @@ std::optional getSignedDistanceFromBoundary( } // If only one of them found the closest bound, return the found lateral distance. if (!rear_lateral_distance_with_idx) { - return front_lateral_distance_with_idx.get().first; + return front_lateral_distance_with_idx.value().first; } else if (!front_lateral_distance_with_idx) { - return rear_lateral_distance_with_idx.get().first; + return rear_lateral_distance_with_idx.value().first; } // If both corners found their closest bound, return the maximum (for left side) or the minimum // (for right side) lateral distance. - double lateral_distance = - left_side - ? std::max( - rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first) - : std::min( - rear_lateral_distance_with_idx.get().first, front_lateral_distance_with_idx.get().first); + double lateral_distance = left_side ? std::max( + rear_lateral_distance_with_idx.value().first, + front_lateral_distance_with_idx.value().first) + : std::min( + rear_lateral_distance_with_idx.value().first, + front_lateral_distance_with_idx.value().first); // Iterate through all segments between the segments closest to the rear and front corners. // Update the lateral distance in case any of these inner segments are closer to the vehicle. - for (size_t i = rear_lateral_distance_with_idx.get().second + 1; - i < front_lateral_distance_with_idx.get().second; i++) { + for (size_t i = rear_lateral_distance_with_idx.value().second + 1; + i < front_lateral_distance_with_idx.value().second; i++) { Pose bound_pose; bound_pose.position = lanelet::utils::conversion::toGeomMsgPt(bound_line_2d[i]); bound_pose.orientation = vehicle_pose.orientation; @@ -1274,7 +1274,7 @@ lanelet::ConstLanelets getCurrentLanesFromPath( const auto & current_pose = planner_data->self_odometry->pose.pose; const auto & p = planner_data->parameters; - std::set lane_ids; + std::set lane_ids; for (const auto & p : path.points) { for (const auto & id : p.lane_ids) { lane_ids.insert(id); @@ -1325,7 +1325,7 @@ lanelet::ConstLanelets extendNextLane( // Add next lane const auto next_lanes = route_handler->getNextLanelets(extended_lanes.back()); if (!next_lanes.empty()) { - boost::optional target_next_lane; + std::optional target_next_lane; if (!only_in_route) { target_next_lane = next_lanes.front(); } @@ -1354,7 +1354,7 @@ lanelet::ConstLanelets extendPrevLane( // Add previous lane const auto prev_lanes = route_handler->getPreviousLanelets(extended_lanes.front()); if (!prev_lanes.empty()) { - boost::optional target_prev_lane; + std::optional target_prev_lane; if (!only_in_route) { target_prev_lane = prev_lanes.front(); } diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp similarity index 97% rename from planning/behavior_path_planner/test/test_drivable_area_expansion.cpp rename to planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp index 99cd5cac2b9d9..2cbaaf46e78cb 100644 --- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp +++ b/planning/behavior_path_planner_common/test/test_drivable_area_expansion.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/data_manager.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/path_projection.hpp" -#include "behavior_path_planner/utils/drivable_area_expansion/types.hpp" +#include "behavior_path_planner_common/data_manager.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/drivable_area_expansion.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/path_projection.hpp" +#include "behavior_path_planner_common/utils/drivable_area_expansion/types.hpp" #include "lanelet2_extension/utility/message_conversion.hpp" #include diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner_common/test/test_safety_check.cpp similarity index 96% rename from planning/behavior_path_planner/test/test_safety_check.cpp rename to planning/behavior_path_planner_common/test/test_safety_check.cpp index a5fc19d1ecbbe..f85f52bae53ad 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner_common/test/test_safety_check.cpp @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/marker_utils/utils.hpp" -#include "behavior_path_planner/utils/path_safety_checker/path_safety_checker_parameters.hpp" -#include "behavior_path_planner/utils/path_safety_checker/safety_check.hpp" +#include "behavior_path_planner_common/marker_utils/utils.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" +#include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include diff --git a/planning/behavior_path_planner/test/test_turn_signal.cpp b/planning/behavior_path_planner_common/test/test_turn_signal.cpp similarity index 99% rename from planning/behavior_path_planner/test/test_turn_signal.cpp rename to planning/behavior_path_planner_common/test/test_turn_signal.cpp index 454be6b75fc73..89101ca5c1ed7 100644 --- a/planning/behavior_path_planner/test/test_turn_signal.cpp +++ b/planning/behavior_path_planner_common/test/test_turn_signal.cpp @@ -11,7 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. -#include "behavior_path_planner/turn_signal_decider.hpp" +#include "behavior_path_planner_common/turn_signal_decider.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 775f3b1c65973..429f6b5a76550 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -69,7 +69,7 @@ BlindSpotModule::BlindSpotModule( turn_direction_(TurnDirection::INVALID), is_over_pass_judge_line_(false) { - velocity_factor_.init(VelocityFactor::REAR_CHECK); + velocity_factor_.init(PlanningBehavior::REAR_CHECK); planner_param_ = planner_param; const auto & assigned_lanelet = @@ -130,7 +130,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto *path = input_path; // reset path return false; } - const size_t closest_idx = closest_idx_opt.get(); + const size_t closest_idx = closest_idx_opt.value(); /* set judge line dist */ const double current_vel = planner_data_->current_velocity->twist.linear.x; @@ -141,7 +141,7 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto stop_point_pose = path->points.at(stop_line_idx).point.pose; const auto ego_segment_idx = motion_utils::findNearestSegmentIndex(input_path.points, current_pose); - if (ego_segment_idx == boost::none) return true; + if (!ego_segment_idx) return true; const size_t stop_point_segment_idx = motion_utils::findNearestSegmentIndex(input_path.points, stop_point_pose.position); const auto distance_until_stop = motion_utils::calcSignedArcLength( @@ -258,8 +258,8 @@ bool BlindSpotModule::generateStopLine( RCLCPP_DEBUG(logger_, "No conflicting line found."); return false; } - stop_idx_ip = - std::max(first_idx_conflicting_lane_opt.get() - 1 - margin_idx_dist - base2front_idx_dist, 0); + stop_idx_ip = std::max( + first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); } else { boost::optional intersection_enter_point_opt = getStartPointFromLaneLet(lane_id_); @@ -269,11 +269,11 @@ bool BlindSpotModule::generateStopLine( } geometry_msgs::msg::Pose intersection_enter_pose; - intersection_enter_pose = intersection_enter_point_opt.get(); + intersection_enter_pose = intersection_enter_point_opt.value(); const auto stop_idx_ip_opt = motion_utils::findNearestIndex(path_ip.points, intersection_enter_pose, 10.0, M_PI_4); if (stop_idx_ip_opt) { - stop_idx_ip = stop_idx_ip_opt.get(); + stop_idx_ip = stop_idx_ip_opt.value(); } stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); @@ -384,8 +384,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( const auto areas_opt = generateBlindSpotPolygons( lanelet_map_ptr, routing_graph_ptr, path, closest_idx, stop_line_pose); if (!!areas_opt) { - debug_data_.detection_areas_for_blind_spot = areas_opt.get().detection_areas; - debug_data_.conflict_areas_for_blind_spot = areas_opt.get().conflict_areas; + debug_data_.detection_areas_for_blind_spot = areas_opt.value().detection_areas; + debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas; autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); @@ -397,8 +397,8 @@ bool BlindSpotModule::checkObstacleInBlindSpot( continue; } - const auto & detection_areas = areas_opt.get().detection_areas; - const auto & conflict_areas = areas_opt.get().conflict_areas; + const auto & detection_areas = areas_opt.value().detection_areas; + const auto & conflict_areas = areas_opt.value().conflict_areas; const bool exist_in_detection_area = std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { return bg::within( @@ -543,7 +543,7 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) : boost::none); if (adj) { - const auto half_lanelet = generateExtendedAdjacentLanelet(adj.get(), turn_direction_); + const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); } } @@ -629,7 +629,7 @@ bool BlindSpotModule::isTargetObjectType( } boost::optional BlindSpotModule::getStartPointFromLaneLet( - const int lane_id) const + const lanelet::Id lane_id) const { lanelet::ConstLanelet lanelet = planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id); @@ -653,7 +653,7 @@ boost::optional BlindSpotModule::getStartPointFromLane lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, - const int lane_id) + const lanelet::Id lane_id) { lanelet::ConstLanelets straight_lanelets; const auto intersection_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index e738fb2e7618a..bb78ed5771d2b 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -201,14 +201,15 @@ class BlindSpotModule : public SceneModuleInterface * @param lane_id lane id of objective lanelet * @return end point of lanelet */ - boost::optional getStartPointFromLaneLet(const int lane_id) const; + boost::optional getStartPointFromLaneLet( + const lanelet::Id lane_id) const; /** * @brief get straight lanelets in intersection */ lanelet::ConstLanelets getStraightLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, - lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id); + lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::Id lane_id); /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. diff --git a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index a4f9e9d7ce23d..762639950b2c5 100644 --- a/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -27,6 +27,7 @@ # param for stuck vehicles stuck_vehicle: + enable_stuck_check_in_intersection: false # [-] flag to enable stuck vehicle checking for crosswalk which is in intersection stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck max_stuck_vehicle_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk diff --git a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp index 6c70e7a7f6dc0..3f4af4a5a1138 100644 --- a/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp +++ b/planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp @@ -82,7 +82,7 @@ struct DebugData std::vector> obj_polygons; }; -std::vector getCrosswalksOnPath( +std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, @@ -105,7 +105,7 @@ std::vector getLinestringIntersects( const geometry_msgs::msg::Point & ego_pos, const size_t max_num); std::optional getStopLineFromMap( - const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name); } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_crosswalk_module/src/manager.cpp index dd8dc95a8ad3c..c743d440bee85 100644 --- a/planning/behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/manager.cpp @@ -63,6 +63,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) cp.no_relax_velocity = getOrDeclareParameter(node, ns + ".slow_down.no_relax_velocity"); // param for stuck vehicle + cp.enable_stuck_check_in_intersection = + getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_stuck_check_in_intersection"); cp.stuck_vehicle_velocity = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity"); cp.max_stuck_vehicle_lateral_offset = @@ -129,8 +131,9 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) const auto rh = planner_data_->route_handler_; const auto launch = [this, &path]( - const auto lane_id, const std::optional & reg_elem_id) { - if (isModuleRegistered(lane_id)) { + const auto road_lanelet_id, const auto crosswalk_lanelet_id, + const std::optional & reg_elem_id) { + if (isModuleRegistered(crosswalk_lanelet_id)) { return; } @@ -141,10 +144,12 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) // NOTE: module_id is always a lane id so that isModuleRegistered works correctly in the case // where both regulatory element and non-regulatory element crosswalks exist. registerModule(std::make_shared( - node_, lane_id, reg_elem_id, lanelet_map_ptr, p, logger, clock_)); - generateUUID(lane_id); + node_, road_lanelet_id, crosswalk_lanelet_id, reg_elem_id, lanelet_map_ptr, p, logger, + clock_)); + generateUUID(crosswalk_lanelet_id); updateRTCStatus( - getUUID(lane_id), true, std::numeric_limits::lowest(), path.header.stamp); + getUUID(crosswalk_lanelet_id), true, std::numeric_limits::lowest(), + path.header.stamp); }; const auto crosswalk_leg_elem_map = planning_utils::getRegElemMapOnPath( @@ -152,14 +157,14 @@ void CrosswalkModuleManager::launchNewModules(const PathWithLaneId & path) for (const auto & crosswalk : crosswalk_leg_elem_map) { // NOTE: The former id is a lane id, and the latter one is a regulatory element's id. - launch(crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); + launch(crosswalk.second.id(), crosswalk.first->crosswalkLanelet().id(), crosswalk.first->id()); } const auto crosswalk_lanelets = getCrosswalksOnPath( planner_data_->current_odometry->pose, path, rh->getLaneletMapPtr(), rh->getOverallGraphPtr()); for (const auto & crosswalk : crosswalk_lanelets) { - launch(crosswalk.id(), std::nullopt); + launch(crosswalk.first, crosswalk.second.id(), std::nullopt); } } diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 56f5072a2ade9..239eddca1fed9 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -148,15 +148,6 @@ StopFactor createStopFactor( return stop_factor; } -std::optional toStdOptional( - const boost::optional & boost_pose) -{ - if (boost_pose) { - return *boost_pose; - } - return std::nullopt; -} - tier4_debug_msgs::msg::StringStamped createStringStampedMessage( const rclcpp::Time & now, const int64_t module_id_, const std::vector> & collision_points) @@ -176,15 +167,16 @@ tier4_debug_msgs::msg::StringStamped createStringStampedMessage( } // namespace CrosswalkModule::CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock) + rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, + const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), module_id_(module_id), planner_param_(planner_param), use_regulatory_element_(reg_elem_id) { - velocity_factor_.init(VelocityFactor::CROSSWALK); + velocity_factor_.init(PlanningBehavior::CROSSWALK); passed_safety_slow_point_ = false; if (use_regulatory_element_) { @@ -200,6 +192,8 @@ CrosswalkModule::CrosswalkModule( crosswalk_ = lanelet_map_ptr->laneletLayer.get(module_id); } + road_ = lanelet_map_ptr->laneletLayer.get(lane_id); + collision_info_pub_ = node.create_publisher("~/debug/collision_info", 1); } @@ -242,8 +236,8 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto std::optional default_stop_pose = std::nullopt; if (p_stop_line.has_value()) { - default_stop_pose = toStdOptional( - calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second)); + default_stop_pose = + calcLongitudinalOffsetPose(path->points, p_stop_line->first, p_stop_line->second); } // Resample path sparsely for less computation cost @@ -486,8 +480,8 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal return std::make_pair(near_attention_range, far_attention_range); } - const auto near_idx = findNearestSegmentIndex(ego_path.points, p_near.get()); - const auto far_idx = findNearestSegmentIndex(ego_path.points, p_far.get()) + 1; + const auto near_idx = findNearestSegmentIndex(ego_path.points, p_near.value()); + const auto far_idx = findNearestSegmentIndex(ego_path.points, p_far.value()) + 1; std::set lane_ids; for (size_t i = near_idx; i < far_idx; ++i) { @@ -575,8 +569,8 @@ std::pair CrosswalkModule::clampAttentionRangeByNeighborCrosswal calcLongitudinalOffsetPoint(ego_path.points, ego_pos, far_attention_range); if (update_p_near && update_p_far) { - debug_data_.range_near_point = update_p_near.get(); - debug_data_.range_far_point = update_p_far.get(); + debug_data_.range_near_point = update_p_near.value(); + debug_data_.range_far_point = update_p_far.value(); } RCLCPP_INFO_EXPRESSION( @@ -732,7 +726,7 @@ void CrosswalkModule::applySafetySlowDownSpeed( const auto & p_safety_slow = calcLongitudinalOffsetPoint(ego_path.points, ego_pos, safety_slow_point_range); - insertDecelPointWithDebugInfo(p_safety_slow.get(), safety_slow_down_speed, output); + insertDecelPointWithDebugInfo(p_safety_slow.value(), safety_slow_down_speed, output); if (safety_slow_point_range < 0.0) { passed_safety_slow_point_ = true; @@ -803,6 +797,16 @@ std::optional CrosswalkModule::checkStopForStuckVehicles( return {}; } + // skip stuck vehicle checking for crosswalk which is in intersection. + if (!p.enable_stuck_check_in_intersection) { + std::string turn_direction = road_.attributeOr("turn_direction", "else"); + if (turn_direction == "right" || turn_direction == "left" || turn_direction == "straight") { + if (!road_.regulatoryElementsAs().empty()) { + return {}; + } + } + } + for (const auto & object : objects) { if (!isVehicle(object)) { continue; diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 0768101179857..ef6d01fc43c23 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -121,6 +121,7 @@ class CrosswalkModule : public SceneModuleInterface double max_slow_down_accel; double no_relax_velocity; // param for stuck vehicle + bool enable_stuck_check_in_intersection{false}; double stuck_vehicle_velocity; double max_stuck_vehicle_lateral_offset; double stuck_vehicle_attention_range; @@ -299,9 +300,10 @@ class CrosswalkModule : public SceneModuleInterface }; CrosswalkModule( - rclcpp::Node & node, const int64_t module_id, const std::optional & reg_elem_id, - const lanelet::LaneletMapPtr & lanelet_map_ptr, const PlannerParam & planner_param, - const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock); + rclcpp::Node & node, const int64_t lane_id, const int64_t module_id, + const std::optional & reg_elem_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const PlannerParam & planner_param, const rclcpp::Logger & logger, + const rclcpp::Clock::SharedPtr clock); bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override; @@ -406,6 +408,8 @@ class CrosswalkModule : public SceneModuleInterface lanelet::ConstLanelet crosswalk_; + lanelet::ConstLanelet road_; + lanelet::ConstLineStrings3d stop_lines_; // Parameter diff --git a/planning/behavior_velocity_crosswalk_module/src/util.cpp b/planning/behavior_velocity_crosswalk_module/src/util.cpp index 47bbef7111da0..ee1b6347c9e73 100644 --- a/planning/behavior_velocity_crosswalk_module/src/util.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/util.cpp @@ -53,19 +53,19 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::Line2d; using tier4_autoware_utils::Point2d; -std::vector getCrosswalksOnPath( +std::vector> getCrosswalksOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { - std::vector crosswalks; + std::vector> crosswalks; // Add current lane id const auto nearest_lane_id = behavior_velocity_planner::planning_utils::getNearestLaneId(path, lanelet_map, current_pose); - std::vector unique_lane_ids; + std::vector unique_lane_ids; if (nearest_lane_id) { // Add subsequent lane_ids from nearest lane_id unique_lane_ids = behavior_velocity_planner::planning_utils::getSubsequentLaneIdsSetOnPath( @@ -81,24 +81,24 @@ std::vector getCrosswalksOnPath( constexpr int PEDESTRIAN_GRAPH_ID = 1; const auto conflicting_crosswalks = overall_graphs->conflictingInGraph(ll, PEDESTRIAN_GRAPH_ID); for (const auto & crosswalk : conflicting_crosswalks) { - crosswalks.push_back(crosswalk); + crosswalks.emplace_back(lane_id, crosswalk); } } return crosswalks; } -std::set getCrosswalkIdSetOnPath( +std::set getCrosswalkIdSetOnPath( const geometry_msgs::msg::Pose & current_pose, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const std::shared_ptr & overall_graphs) { - std::set crosswalk_id_set; + std::set crosswalk_id_set; for (const auto & crosswalk : getCrosswalksOnPath(current_pose, path, lanelet_map, overall_graphs)) { - crosswalk_id_set.insert(crosswalk.id()); + crosswalk_id_set.insert(crosswalk.second.id()); } return crosswalk_id_set; @@ -209,7 +209,7 @@ std::vector getLinestringIntersects( } std::optional getStopLineFromMap( - const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, + const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr, const std::string & attribute_name) { lanelet::ConstLanelet lanelet = lanelet_map_ptr->laneletLayer.get(lane_id); diff --git a/planning/behavior_velocity_detection_area_module/src/scene.cpp b/planning/behavior_velocity_detection_area_module/src/scene.cpp index 4d635d3d6d641..2815aa52c52e7 100644 --- a/planning/behavior_velocity_detection_area_module/src/scene.cpp +++ b/planning/behavior_velocity_detection_area_module/src/scene.cpp @@ -47,7 +47,7 @@ DetectionAreaModule::DetectionAreaModule( state_(State::GO), planner_param_(planner_param) { - velocity_factor_.init(VelocityFactor::USER_DEFINED_DETECTION_AREA); + velocity_factor_.init(PlanningBehavior::USER_DEFINED_DETECTION_AREA); } LineString2d DetectionAreaModule::getStopLineGeometry2d() const @@ -110,7 +110,7 @@ bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * return false; } - modified_stop_pose = ego_pos_on_path.get(); + modified_stop_pose = ego_pos_on_path.value(); modified_stop_line_seg_idx = current_seg_idx; } diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 3ed4ee8fd334a..12455a26d2f4a 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -1,46 +1,67 @@ -## Intersection +# Intersection -### Role +## Role -The _intersection_ module is responsible for safely going through urban intersections by: +The intersection module is responsible for safely passing urban intersections by: 1. checking collisions with upcoming vehicles 2. recognizing the occluded area in the intersection -3. reacting to arrow signals of associated traffic lights +3. reacting to each color/shape of associated traffic lights -The module is designed to be agnostic to left-hand/right-hand traffic rules and works on crossroads, T-shape junctions, etc. +This module is designed to be agnostic to left-hand/right-hand traffic rules and work for crossroads, T-shape junctions, etc. Roundabout is not formally supported in this module. ![topology](./docs/intersection-topology.drawio.svg) -### Activation condition +## Activation condition -This module is activated when the path contains the lanes with `turn_direction` tag. More precisely, if the `lane_ids` of the path contains the ids of those lanes, corresponding instances of intersection module are activated on each lanes respectively. +This module is activated when the path contains the lanes with turn_direction tag. More precisely, if the lane_ids of the path contain the ids of those lanes, corresponding instances of intersection module are activated on each lane respectively. -### Requirements/Limitations +## Requirements/Limitations -- The HDMap needs to have the information of `turn_direction` tag (which should be one of `straight`, `left`, `right`) for all the lanes in intersections and `right_of_way` tag for specific lanes (refer to [RightOfWay](#right-of-way) section for more details). See [lanelet2_extension document](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) for more detail. +- The HDMap needs to have the information of turn_direction tag (which should be one of straight, left, right) for all the lanes in intersections and right_of_way tag for specific lanes (refer to [RightOfWay](#how-towhy-set-rightofway-tag) section for more details). See [lanelet2_extension document](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) for more detail. - WIP(perception requirements/limitations) - WIP(sensor visibility requirements/limitations) -### Attention area - -The `attention area` in the intersection are defined as the set of lanes that are conflicting with ego vehicle's path and their preceding lanes up to `common.attention_area_length` meters. `RightOfWay` tag is used to rule out the lanes that each lane has priority given the traffic light relation and `turn_direction` priority(`yield lane`). +## Attention area -`Intersection Area`, which is supposed to be defined on the HDMap, is an area converting the entire intersection. +The attention area in the intersection is defined as the set of lanes that are conflicting with ego path and their preceding lanes up to `common.attention_area_length` meters. By default RightOfWay tag is not set, so the attention area covers all the conflicting lanes and its preceding lanes as shown in the first row. RightOfWay tag is used to rule out the lanes that each lane has priority given the traffic light relation and turn_direction priority. In the second row, purple lanes are set as the yield_lane of the ego_lane in the RightOfWay tag. ![attention_area](./docs/intersection-attention.drawio.svg) -#### Right Of Way +intersection_area, which is supposed to be defined on the HDMap, is an area converting the entire intersection. + +### In-phase/Anti-phase signal group + +The terms "in-phase signal group" and "anti-phase signal group" are introduced to distinguish the lanes by the timing of traffic light regulation as shown in below figure. + +![phase signal group](./docs/signal-phase-group.drawio.svg) + +The set of intersection lanes whose color is in sync with lane L1 is called the in-phase signal group of L1, and the set of remaining lanes is called the anti-phase signal group. + +### How-to/Why set RightOfWay tag + +Ideally RightOfWay tag is unnecessary if ego has perfect knowledge of all traffic signal information because: -Following table shows an example of how to assign `right_of_way` tag and set `yield_lanes` to each lane in intersections. +- it can distinguish which conflicting lanes should be checked because they are GREEN currently and possible collision occur with the vehicles on those lanes +- it can distinguish which conflicting lanes can be ignored because they are RED currently and there is no chance of collision with the vehicles on those lanes unless they violate the traffic rule -| turn direction / traffic light | w/ traffic light | w/o traffic light | -| ------------------------------ | --------------------------------------------------------------- | ------------------------------------------------ | -| straight | Highest priority of all | Priority over left/right lanes of the same group | -| left(Left hand traffic) | Priority over the other group and right lanes of the same group | Priority over right lanes of the same group | -| right(Left hand traffic) | Priority only over the other group | priority only over the other group | -| left(Right hand traffic) | Priority only over the other group | Priority only over the other group | -| right(Right hand traffic) | Priority over the other group and left lanes of the same group | priority over left lanes of the same group | +That allows ego to generate the attention area dynamically using the real time traffic signal information. However this ideal condition rarely holds unless the traffic signal information is provided through the infrastructure. Also there maybe be very complicated/bad intersection maps where multiple lanes overlap in a complex manner. + +- If there is an perfect access to entire traffic light signal, then you can set `common.use_map_right_of_way` to false and there is no need to set RightOfWay tag on the map. The intersection module will generate the attention area by checking traffic signal and corresponding conflicting lanes. This feature is not implemented yet. +- If traffic signal information is not perfect, then set `common.use_map_right_of_way` to true. If you do not want to detect vehicles on the anti-phase signal group lanes, set them as yield_lane for ego lane. +- Even if there are no traffic lights if the intersection lanes are overlapped in a ugly manner, you may need to set RightOfWay tag. For example if adjacent intersection lanes of the same in-phase group are not sharing the boundary line and overlapped a little bit, you may need to set RightOfWay to each other for them in order to avoid unnecessary stop for vehicle on such unrelated lane. + +To help the intersection module care only a set of limited lanes, RightOfWay tag needs to be properly set. + +Following table shows an **example** of how to set yield_lanes to each lane in a intersection w/o traffic lights. Since it is not apparent how to uniquely determine signal phase group for a set of intersection lanes in geometric/topological manner, yield_lane needs to be set manually. Straight lanes with traffic lights are exceptionally handled to detect no lanes because commonly it has priority over all the other lanes, so no RightOfWay setting is required. + +| turn direction of right_of_way | yield_lane(with traffic light) | yield_lane(without traffic light) | +| ------------------------------ | ------------------------------------------------------------------------------------------- | ---------------------------------------------- | +| straight | not need to set yield_lane(this case is special) | left/right conflicting lanes of in-phase group | +| left(Left hand traffic) | all conflicting lanes of the anti-phase group and right conflicting lanes of in-phase group | right conflicting lanes of in-phase group | +| right(Left hand traffic) | all conflicting lanes of the anti-phase group | no yield_lane | +| left(Right hand traffic) | all conflicting lanes of the anti-phase group | no yield_lane | +| right(Right hand traffic) | all conflicting lanes of the anti-phase group and right conflicting lanes of in-phase group | left conflicting lanes of in-phase group | This setting gives the following `attention_area` configurations. @@ -48,45 +69,141 @@ This setting gives the following `attention_area` configurations. ![attention_area_ll_rr](./docs/intersection-attention-ll-rr.drawio.svg) ![attention_area_lr_rl](./docs/intersection-attention-lr-rl.drawio.svg) -### Target objects +For complex/bad intersection map like the one illustrated below, additional RightOfWay setting maybe necessary. + +![bad-map](./docs/ugly-intersection.drawio.svg) + +The bad points are: + +1. ego lane is overlapped with adjacent lane of the in-phase group. In this case you need to set this lane as yield_lane additionally because otherwise attention area is generated for its preceding lanes as well, which may cause unwanted stop. +2. ego lane is overlapped with unrelated lane. In this case the lane is right-turn only and there is no chance of collision in theory. But you need to set this lane as yield_lane additionally for the same reason as (1). + +## Possible stop lines + +Following figure illustrates important positions used in the intersection module. Note that each solid line represents ego front line position and the corresponding dot represents the actual inserted stop point position for the vehicle frame, namely the center of the rear wheel. + +![data structure](./docs/intersection-stoplines.drawio.svg) + +To precisely calculate stop positions, the path is interpolated at the certain interval of `common.path_interpolation_ds`. + +- closest_idx denotes the path point index which is closest to ego position. +- first_attention_stopline denotes the first path point where ego footprint intersects with the attention_area. +- If a stopline is associated with the intersection lane on the map, that line is used as the default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as the default_stopline instead. +- occlusion_peeking_stopline is a bit ahead of first_attention_stopline as described later. +- occlusion_wo_tl_pass_judge_line is the first position where ego footprint intersects with the centerline of the first attention_area lane. + +## Target objects For [stuck vehicle detection](#stuck-vehicle-detection) and [collision detection](#collision-detection), this module checks **car**, **bus**, **truck**, **trailer**, **motor cycle**, and **bicycle** type objects. Objects that satisfy all of the following conditions are considered as target objects (possible collision objects): -- The center of mass of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . - - (Optional condition) The center of gravity is in the **intersection area**. +- The center of the object is **within a certain distance** from the attention lane (threshold = `common.attention_area_margin`) . + - (Optional condition) The center of the object is in the **intersection area**. - To deal with objects that is in the area not covered by the lanelets in the intersection. - The posture of object is **the same direction as the attention lane** (threshold = `common.attention_area_angle_threshold`). -- Not being **in the adjacent lanes of the ego vehicle**. +- Not being **in the adjacent lanes of ego**. + +## Overview of decision process + +There are several behaviors depending on the scene. + +| behavior | scene | action | +| ------------------------ | ------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------- | +| Safe | Ego detected no occlusion and collision | Ego passes the intersection | +| StuckStop | The exit of the intersection is blocked by traffic jam | Ego stops before the intersection or the boundary of attention area | +| YieldStuck | Another vehicle stops to yield ego | Ego stops before the intersection or the boundary of attention area | +| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at the default_stop_line | +| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at the default_stop_line at first | +| PeekingTowardOcclusion | Ego detected occlusion and but no collision within the FOV (after FirstWaitBeforeOcclusion) | Ego approaches the boundary of the attention area slowly | +| OccludedCollisionStop | Ego detected both occlusion and collision (after FirstWaitBeforeOcclusion) | Ego stops immediately | +| FullyPrioritized | Ego is fully prioritized by the RED/Arrow signal | Ego only cares vehicles still running inside the intersection. Occlusion is ignored | +| OverPassJudgeLine | Ego is already inside the attention area and/or cannot stop before the boundary of attention area | Ego does not detect collision/occlusion anymore and passes the intersection | + +```plantuml +@startuml + +state begin <> +[*] --> begin +begin --> OverPassJudgeLine: IF over_pass_judge + +state "Before pass judge line" as NotOverPassJudgeLine { +state check_stuck <> +begin --> check_stuck: ELSE + +check_stuck --> StuckStop: IF stuck vehicle detected + +state check_yield_stuck <> +check_stuck --> check_yield_stuck: ELSE +check_yield_stuck --> YieldStuck: IF yield stuck vehicle detected + +state check_tl_priority <> +check_yield_stuck --> check_tl_priority: ELSE -#### Stuck Vehicle Detection +state check_occlusion <> +check_tl_priority --> check_occlusion: IF not prioritized -If there is any object on the path in inside the intersection and at the exit of the intersection (up to `stuck_vehicle_detect_dist`) lane and its velocity is less than a threshold (`stuck_vehicle.stuck_vehicle_vel_thr`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`stop_line_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the vehicle path, so the stuck vehicle stopline is not inserted if the upstream module generated avoidance path +state Safe +State "Prioritized by traffic light" as Prioritized { +state check_collision_prioritized <> +check_tl_priority --> check_collision_prioritized: IF prioritized +State FullyPrioritized +check_collision_prioritized --> FullyPrioritized: IF collision detected +check_collision_prioritized --> Safe: ELSE +} + +check_occlusion --> Occlusion: IF occlusion is detected + +State "Occlusion is not detected" as NoOcclusion { +state check_collision <> +check_occlusion --> check_collision: ELSE +State NonOccludedCollisionStop +check_collision --> Safe: ELSE +check_collision --> NonOccludedCollisionStop: IF collision detected +} + +State "Occlusion is detected" as Occlusion { +state FirstWaitBeforeOcclusion +FirstWaitBeforeOcclusion --> Peeking: after termporal stop +state Peeking { +State PeekingTowardOcclusion +State OccludedCollisionStop +PeekingTowardOcclusion --> OccludedCollisionStop : IF collision detected +OccludedCollisionStop --> PeekingTowardOcclusion: IF not collision detected +} +} + +} + +@enduml +``` + +## Stuck Vehicle Detection + +If there is any object on the path inside the intersection and at the exit of the intersection (up to `stuck_vehicle.stuck_vehicle_detect_dist`) lane and its velocity is less than the threshold (`stuck_vehicle.stuck_vehicle_velocity_threshold`), the object is regarded as a stuck vehicle. If stuck vehicles exist, this module inserts a stopline a certain distance (=`default_stopline_margin`) before the overlapped region with other lanes. The stuck vehicle detection area is generated based on the planned path, so the stuck vehicle stopline is not inserted if the upstream module generated an avoidance path. ![stuck_vehicle_detection](./docs/stuck-vehicle.drawio.svg) -#### Collision detection +## Collision detection -The following process is performed for the targets objects to determine whether the ego vehicle can pass the intersection safely. If it is judged that the ego vehicle cannot pass through the intersection with enough margin, this module inserts a stopline on the path. +The following process is performed for the targets objects to determine whether ego can pass the intersection safely. If it is judged that ego cannot pass the intersection with enough margin, this module inserts a stopline on the path. -1. calculate the time interval that the ego vehicle is in the intersection. This time is set as $t_s$ ~ $t_e$ -2. extract the predicted path of the target object whose confidence is greater than `collision_detection.min_predicted_path_confidence`. -3. detect collision between the extracted predicted path and ego's predicted path in the following process. - 1. obtain the passing area of the ego vehicle $A_{ego}$ in $t_s$ ~ $t_e$. - 2. calculate the passing area of the target object $A_{target}$ at $t_s$ - `collision_detection.collision_start_margin_time` ~ $t_e$ + `collision_detection.collision_end_margin_time` for each predicted path (\*1). - 3. check if $A_{ego}$ and $A_{target}$ polygons are overlapped (has collision). -4. when a collision is detected, the module inserts a stopline. -5. If ego is over the `pass_judge_line`, collision checking is not processed to avoid sudden braking and/or unnecessary stop in the middle of the intersection. +1. predict the time $t$ when the object intersects with ego path for the first time from the predicted path time step. Only the predicted whose confidence is greater than `collision_detection.min_predicted_path_confidence` is used. +2. detect collision between the predicted path and ego's predicted path in the following process + 1. calculate the collision interval of [$t$ - `collision_detection.collision_start_margin_time`, $t$ + `collision_detection.collision_end_margin_time`] + 2. calculate the passing area of ego during the collision interval from the array of (time, distance) obtained by smoothed velocity profile + 3. check if ego passing area and object predicted path interval collides +3. if collision is detected, the module inserts a stopline +4. if ego is over the [pass_judge_line](#pass-judge-line), collision checking is skipped to avoid sudden braking and/or unnecessary stop in the middle of the intersection The parameters `collision_detection.collision_start_margin_time` and `collision_detection.collision_end_margin_time` can be interpreted as follows: -- If the ego vehicle was to pass through the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. -- If the ego vehicle was to pass through the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. +- If ego was to pass the intersection earlier than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_start_margin_time`. +- If ego was to pass the intersection later than the target object, collision would be detected if the time difference between the two was less than `collision_detection.collision_end_margin_time`. -If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions. +If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.collision_detection_hold_time` to prevent the chattering of decisions. -Currently, the intersection module uses `motion_velocity_smoother` feature to precisely calculate ego vehicle velocity profile along the intersection lane under longitudinal/lateral constraints. If the flag `collision_detection.use_upstream_velocity` is true, the target velocity profile of the original path is used. Otherwise the target velocity is set to `common.intersection_velocity`. In the trajectory smoothing process the target velocity at/before ego trajectory points are set to ego current velocity. The smoothed trajectory is then converted to an array of (time, distance) which indicates the arrival time to each trajectory point on the path from current ego position. You can visualize this array by adding the lane id to `debug.ttc` and running +Currently, the intersection module uses `motion_velocity_smoother` feature to precisely calculate ego velocity profile along the intersection lane under longitudinal/lateral constraints. If the flag `collision_detection.velocity_profile.use_upstream` is true, the target velocity profile of the original path is used. Otherwise the target velocity is set to `collision.velocity_profile.default_velocity`. In the trajectory smoothing process the target velocity at/before ego trajectory points are set to ego current velocity. The smoothed trajectory is then converted to an array of (time, distance) which indicates the arrival time to each trajectory point on the path from current ego position. You can visualize this array by adding the lane id to `debug.ttc` and running ```bash ros2 run behavior_velocity_intersection_module ttc.py --lane_id @@ -94,42 +211,88 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id ![ego ttc profile](./docs/ttc.gif) -#### Stop Line Automatic Generation +## Occlusion detection -If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front. +If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough ego first makes a brief stop at the default stop line for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stop_line. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stop_line. Otherwise only stop line is inserted. -#### Pass Judge Line +During the creeping if collision is detected this module inserts a stop line in front of ego immediately, and if the FOV gets sufficiently clear the intersection_occlusion wall will disappear. If occlusion is cleared and no collision is detected ego will pass the intersection. -To avoid sudden braking, if deceleration and jerk more than a threshold (`behavior_velocity_planner.max_accel` and `behavior_velocity_planner.max_jerk`) is required to stop just in front of the attention area, namely the `first_attention_stop_line`, this module does not insert stopline after it passed the `default stop_line` position. +The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from ego path along the lane as shown below. -The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. +![occlusion_detection](./docs/occlusion_grid.drawio.svg) -- If `occlusion.enable` is false, the pass judge line before the `first_attention_stop_line` by the braking distance $v_{ego}^{2} / 2a_{max}$. -- If `occlusion.enable` is true and: - - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stop_line` in order to continue peeking/collision detection while occlusion is detected. - - if there are no associated traffic lights and: - - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. - - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. +If the nearest occlusion cell value is below the threshold `occlusion.occlusion_required_clearance_distance`, it means that the FOV of ego is not clear. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. -![data structure](./docs/data-structure.drawio.svg) +### Occlusion source estimation at intersection with traffic light -### Occlusion detection +At intersection with traffic light, the whereabout of occlusion is estimated by checking if there are any objects between ego and the nearest occlusion cell. While the occlusion is estimated to be caused by some object (DYNAMICALLY occluded), intersection_wall appears at all times. If no objects are found between ego and the nearest occlusion cell (STATICALLY occluded), after ego stopped for the duration of `occlusion.static_occlusion_with_traffic_light_timeout` plus `occlusion.occlusion_detection_hold_time`, occlusion is intentionally ignored to avoid stuck. -If the flag `occlusion.enable` is true this module checks if there is sufficient field of view (FOV) on the attention area up to `occlusion.occlusion_attention_area_length`. If FOV is not clear enough the ego vehicle will once stop at the _default stop line_ for `occlusion.before_creep_stop_time`, and then slowly creep toward _occlusion peeking stop line_ at the speed of `occlusion.occlusion_creep_velocity` if `occlusion.enable_creeping` is true. During the creeping if collision is detected this module inserts a stop line instantly, and if the FOV gets sufficiently clear the _intersection occlusion_ wall will disappear. If occlusion is cleared and no collision is detected the ego vehicle will pass the intersection. +![occlusion_detection](./docs/occlusion-with-tl.drawio.svg) -The occlusion is detected as the common area of occlusion attention area(which is partially the same as the normal attention area) and the unknown cells of the occupancy grid map. The occupancy grid map is denoised using morphology with the window size of `occlusion.denoise_kernel`. The occlusion attention area lanes are discretized to line strings and they are used to generate a grid whose each cell represents the distance from the ego path along the lane as shown below. +The remaining time is visualized on the intersection_occlusion virtual wall. -![occlusion_detection](./docs/occlusion_grid.drawio.svg) +![static-occlusion-timeout](./docs/static-occlusion-timeout.png) -If the nearest occlusion cell value is below the threshold, occlusion is detected. It is expected that the occlusion gets cleared as the vehicle approaches the occlusion peeking stop line. +### Occlusion handling at intersection without traffic light -In there are no traffic lights associated with the lane, the ego vehicle will make a brief stop at the _default stop line_ and the position where the vehicle heading touches the attention area for the first time(which is denoted as _first attention stop line_). After stopping at the _first attention area stop line_ this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and the position which is `occlusion.absence_traffic_light.maximum_peeking_distance` ahead of _first attention area stop line_ while occlusion is not cleared. If collision is detected, ego will instantly stop. Once the occlusion is cleared or ego passed `occlusion.absence_traffic_light.maximum_peeking_distance` this module does not detect collision and occlusion because ego vehicle is already inside the intersection. +At intersection without traffic light, if occlusion is detected, ego makes a brief stop at the default_stopline and first_attention_stopline respectively. After stopping at the first_attention_area_stopline this module inserts `occlusion.absence_traffic_light.creep_velocity` velocity between ego and occlusion_wo_tl_pass_judge_line while occlusion is not cleared. If collision is detected, ego immediately stops. Once the occlusion is cleared or ego has passed occlusion_wo_tl_pass_judge_line this module does not detect collision and occlusion because ego footprint is already inside the intersection. ![occlusion_detection](./docs/occlusion-without-tl.drawio.svg) -### Data Structure +While ego is creeping, yellow intersection_wall appears in front ego. + +![occlusion-wo-tl-creeping](./docs/occlusion-wo-tl-creeping.png) + +## Traffic signal specific behavior + +### Collision detection + +TTC parameter varies depending on the traffic light color/shape as follows. + +| traffic light color | ttc(start) | ttc(end) | +| ------------------- | ---------------------------------------------------------------------- | ---------------------------------------------------------------------- | +| GREEN | `collision_detection.not_prioritized.collision_start_margin` | `collision_detection.not_prioritized.collision_end_margin` | +| AMBER | `collision_detection.partially_prioritized.collision_start_end_margin` | `collision_detection.partially_prioritized.collision_start_end_margin` | +| RED / Arrow | `collision_detection.fully_prioritized.collision_start_end_margin` | `collision_detection.fully_prioritized.collision_start_end_margin` | + +### yield on GREEN + +If the traffic light color changed to GREEN and ego approached the entry of the intersection lane within the distance `collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start` and there is any object whose distance to its stopline is less than `collision_detection.yield_on_green_traffic_light.object_dist_to_stopline`, this module commands to stop for the duration of `collision_detection.yield_on_green_traffic_light.duration` at the default_stopline. + +### skip on AMBER + +If the traffic light color is AMBER but the object is expected to stop before its stopline under the deceleration of `collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration`, collision checking is skipped. + +### skip on RED -#### `IntersectionLanelets` +If the traffic light color is RED or Arrow signal is turned on, the attention lanes which are not conflicting with ego lane are not used for detection. And even if the object stops with a certain overshoot from its stopline, but its expected stop position under the deceleration of `collision_detection.ignore_on_amber_traffic_light.object_expected_deceleration` is more than the distance `collision_detection.ignore_on_red_traffic_light.object_margin_to_path` from collision point, the object is ignored. + +### Occlusion detection + +When the traffic light color/shape is RED/Arrow, occlusion detection is skipped. + +## Pass Judge Line + +To avoid sudden braking, if deceleration and jerk more than the threshold (`common.max_accel` and `common.max_jerk`) is required to stop at first_attention_stopline, this module does not command to stop once it passed the default_stopline position. + +If ego passed pass_judge_line, then ego does not stop anymore. If ego passed pass_judge_line while ego is stopping for dangerous decision, then ego stops while the situation is judged as dangerous. Once the judgement turned safe, ego restarts and does not stop anymore. + +The position of the pass judge line depends on the occlusion detection configuration and the existence of the associated traffic light of the intersection lane. + +- If `occlusion.enable` is false, the pass judge line before the `first_attention_stopline` by the braking distance $v_{ego}^{2} / 2a_{max}$. +- If `occlusion.enable` is true and: + - if there are associated traffic lights, the pass judge line is at the `occlusion_peeking_stopline` in order to continue peeking/collision detection while occlusion is detected. + - if there are no associated traffic lights and: + - if occlusion is detected, pass judge line is at the `occlusion_wo_tl_pass_judge_line` to continue peeking. + - if occlusion is not detected, pass judge line is at the same place at the case where `occlusion.enable` is false. + +## Data Structure + +Each data structure is defined in `util_type.hpp`. + +![data-structure](./docs/data-structure.drawio.svg) + +### `IntersectionLanelets` ```plantuml @startuml @@ -148,12 +311,12 @@ entity IntersectionLanelets { Part of attention lanes/area for occlusion detection -- * is_priortized: bool - If ego vehicle has priority in current traffic light context + If ego has priority in current traffic light context } @enduml ``` -#### `IntersectionStopLines` +### `IntersectionStopLines` Each stop lines are generated from interpolated path points to obtain precise positions. @@ -161,62 +324,172 @@ Each stop lines are generated from interpolated path points to obtain precise po @startuml entity IntersectionStopLines { * closest_idx: size_t - closest path point index for ego vehicle + closest path point index for ego -- - * stuck_stop_line: size_t + * stuck_stopline: size_t stop line index on stuck vehicle detection -- - * default_stop_line: size_t - If defined on the map, its index on the path. Otherwise generated before first_attention_stop_line + * default_stopline: size_t + If defined on the map, its index on the path. Otherwise generated before first_attention_stopline -- - * first_attention_stop_line + * first_attention_stopline The index of the first path point which is inside the attention area -- - * occlusion_peeking_stop_line + * occlusion_peeking_stopline The index of the path point for the peeking limit position -- * pass_judge_line - The index of the path point which is before first_attention_stop_line/occlusion_peeking_stop_line by braking distance + The index of the path point which is before first_attention_stopline/occlusion_peeking_stopline by braking distance } @enduml ``` -### Module Parameters +### `TargetObject` -| Parameter | Type | Description | -| --------------------------------------------------- | ------ | ---------------------------------------------------------------------------------------------- | -| `common.attention_area_margin` | double | [m] margin for expanding attention area width | -| `common.attention_area_length` | double | [m] range for object detection | -| `common.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | -| `common.stop_line_margin` | double | [m] margin before stop line | -| `common.intersection_velocity` | double | [m/s] velocity profile for pass judge calculation | -| `common.intersection_max_accel` | double | [m/s^2] acceleration profile for pass judge calculation | -| `common.stop_overshoot_margin` | double | [m] margin for the overshoot from stopline | -| `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | -| `stuck_vehicle.stuck_vehicle_vel_thr` | double | [m/s] velocity threshold for stuck vehicle detection | -| `collision_detection.state_transit_margin_time` | double | [m] time margin to change state | -| `collision_detection.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | -| `collision_detection.collision_start_margin_time` | double | [s] time margin for the beginning of collision with upcoming vehicle | -| `collision_detection.collision_end_margin_time` | double | [s] time margin for the finish of collision with upcoming vehicle | -| `collision_detection.keep_detection_vel_thr` | double | [m/s] ego velocity threshold for continuing collision detection before pass judge line | -| `occlusion.occlusion_attention_area_length` | double | [m] the length of attention are for occlusion detection | -| `occlusion.enable_creeping` | bool | [-] flag to insert `occlusion_creep_velocity` while peeking to intersection occlusion stopline | -| `occlusion.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | -| `occlusion.min_vehicle_brake_for_rss` | double | [m/s] assumed minimum brake of the vehicle running from behind the occlusion | -| `occlusion.max_vehicle_velocity_for_rss` | double | [m/s] assumed maximum velocity of the vehicle running from behind the occlusion | - -#### For developers only - -| Parameter | Type | Description | -| ------------------------------ | ------ | ---------------------------------------------------------------------- | -| `common.path_interpolation_ds` | double | [m] path interpolation interval | -| `occlusion.denoise_kernel` | double | [m] the window size of morphology process for clearing noisy occlusion | - -### How to turn parameters +`TargetObject` holds the object, its belonging lane and corresponding stopline information. -WIP +```plantuml +@startuml +entity TargetObject { + * object: PredictedObject + detected object + -- + * attention_lanelet: ConstLanelet + belonging lanelet instance + -- + * stopline: ConstLineString3d + reachable stopline of attention_lanelet +} +@enduml +``` + +## Module Parameters + +### common + +| Parameter | Type | Description | +| --------------------------------- | ------ | ------------------------------------------------------------------------ | +| `.attention_area_length` | double | [m] range for object detection | +| `.attention_area_margin` | double | [m] margin for expanding attention area width | +| `.attention_area_angle_threshold` | double | [rad] threshold of angle difference between the detected object and lane | +| `.use_intersection_area` | bool | [-] flag to use intersection_area for collision detection | +| `.default_stopline_margin` | double | [m] margin before_stop_line | +| `.stopline_overshoot_margin` | double | [m] margin for the overshoot from stopline | +| `.max_accel` | double | [m/ss] max acceleration for stop | +| `.max_jerk` | double | [m/sss] max jerk for stop | +| `.delay_response_time` | double | [s] action delay before stop | + +### stuck_vehicle/yield_stuck + +| Parameter | Type | Description | +| ------------------------------------------------ | ------ | ---------------------------------------------------------------------------- | +| `stuck_vehicle.turn_direction` | - | [-] turn_direction specifier for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_detect_dist` | double | [m] length toward from the exit of intersection for stuck vehicle detection | +| `stuck_vehicle.stuck_vehicle_velocity_threshold` | double | [m/s] velocity threshold for stuck vehicle detection | +| `yield_stuck.distance_threshold` | double | [m/s] distance threshold of yield stuck vehicle from ego path along the lane | + +### collision_detection + +| Parameter | Type | Description | +| --------------------------------------------- | ------ | ------------------------------------------------------------------------------------ | +| `.consider_wrong_direction_vehicle` | bool | [-] flag to detect objects in the wrong direction | +| `.collision_detection_hold_time` | double | [s] hold time of collision detection | +| `.min_predicted_path_confidence` | double | [-] minimum confidence value of predicted path to use for collision detection | +| `.keep_detection_velocity_threshold` | double | [s] ego velocity threshold for continuing collision detection before pass judge line | +| `.velocity_profile.use_upstream` | bool | [-] flag to use velocity profile planned by upstream modules | +| `.velocity_profile.minimum_upstream_velocity` | double | [m/s] minimum velocity of upstream velocity profile to avoid zero division | +| `.velocity_profile.default_velocity` | double | [m/s] constant velocity profile when use_upstream is false | +| `.velocity_profile.minimum_default_velocity` | double | [m/s] minimum velocity of default velocity profile to avoid zero division | +| `.yield_on_green_traffic_light` | - | [-] [description](#yield-on-green) | +| `.ignore_amber_traffic_light` | - | [-] [description](#skip-on-amber) | +| `.ignore_on_red_traffic_light` | - | [-] [description](#skip-on-red) | + +### occlusion + +| Parameter | Type | Description | +| ---------------------------------------------- | -------- | ------------------------------------------------------------------------------------------- | +| `.enable` | bool | [-] flag to calculate occlusion detection | +| `.occlusion_attention_area_length` | double | [m] the length of attention are for occlusion detection | +| `.free_space_max` | int | [-] maximum value of occupancy grid cell to treat at occluded | +| `.occupied_min` | int | [-] minimum value of occupancy grid cell to treat at occluded | +| `.denoise_kernel` | double | [m] morphology window size for preprocessing raw occupancy grid | +| `.attention_lane_crop_curvature_threshold` | double | [m] curvature threshold for trimming curved part of the lane | +| `.attention_lane_crop_curvature_ds` | double | [m] discretization interval of centerline for lane curvature calculation | +| `.creep_during_peeking.enable` | bool | [-] flag to insert `creep_velocity` while peeking to intersection occlusion stopline | +| `.creep_during_peeking.creep_velocity` | double | [m/s] the command velocity while peeking to intersection occlusion stopline | +| `.peeking_offset` | double | [m] the offset of the front of the vehicle into the attention area for peeking to occlusion | +| `.occlusion_required_clearance_distance` | double | [m] threshold for the distance to nearest occlusion cell from ego path | +| `.possible_object_bbox` | [double] | [m] minimum bounding box size for checking if occlusion polygon is small enough | +| `.ignore_parked_vehicle_speed_threshold` | double | [m/s] velocity threshold for checking parked vehicle | +| `.occlusion_detection_hold_time` | double | [s] hold time of occlusion detection | +| `.temporal_stop_time_before_peeking` | double | [s] temporal stop duration at the default_stop_line before starting peeking | +| `.temporal_stop_before_attention_area` | bool | [-] flag to temporarily stop at first_attention_stopline before peeking into attention_area | +| `.creep_velocity_without_traffic_light` | double | [m/s] creep velocity to occlusion_wo_tl_pass_judge_line | +| `.static_occlusion_with_traffic_light_timeout` | double | [s] the timeout duration for ignoring static occlusion at intersection with traffic light | + +## Trouble shooting + +### Intersection module stops against unrelated vehicles + +In this case, first visualize `/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection` topic and check the `attention_area` polygon. Intersection module performs collision checking for vehicles running on this polygon, so if it extends to unintended lanes, it needs to have [RightOfWay tag](#how-towhy-set-rightofway-tag). + +By lowering `common.attention_area_length` you can check which lanes are conflicting with the intersection lane. Then set part of the conflicting lanes as the yield_lane. + +### The stop line of intersection is chattering + +The parameter `collision_detection.collision_detection_hold_time` suppresses the chattering by keeping UNSAFE decision for this duration until SAFE decision is finally made. The role of this parameter is to account for unstable detection/tracking of objects. By increasing this value you can suppress the chattering. However it could elongate the stopping duration excessively. + +If the chattering arises from the acceleration/deceleration of target vehicles, increase `collision_detection.collision_detection.collision_end_margin_time` and/or `collision_detection.collision_detection.collision_end_margin_time`. + +### The stop line is released too fast/slow + +If the intersection wall appears too fast, or ego tends to stop too conservatively for upcoming vehicles, lower the parameter `collision_detection.collision_detection.collision_start_margin_time`. If it lasts too long after the target vehicle passed, then lower the parameter `collision_detection.collision_detection.collision_end_margin_time`. + +### Ego suddenly stops at intersection with traffic light + +If the traffic light color changed from AMBER/RED to UNKNOWN, the intersection module works in the GREEN color mode. So collision and occlusion are likely to be detected again. + +### Occlusion is detected overly + +You can check which areas are detected as occlusion by visualizing `/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/occlusion_polygons`. + +If you do not want to detect / do want to ignore occlusion far from ego or lower the computational cost of occlusion detection, `occlusion.occlusion_attention_area_length` should be set to lower value. + +If you want to care the occlusion nearby ego more cautiously, set `occlusion.occlusion_required_clearance_distance` to a larger value. Then ego will approach the occlusion_peeking_stopline more closely to assure more clear FOV. + +`occlusion.possible_object_bbox` is used for checking if detected occlusion area is small enough that no vehicles larger than this size can exist inside. By decreasing this size ego will ignore small occluded area. + +#### occupancy grid map tuning + +Refer to the document of [probabilistic_occupancy_grid_map](https://autowarefoundation.github.io/autoware.universe/main/perception/probabilistic_occupancy_grid_map/) for details. If occlusion tends to be detected at apparently free space, increase `occlusion.free_space_max` to ignore them. + +#### in simple_planning_simulator + +intersection_occlusion feature is **not recommended** for use in planning_simulator because the laserscan_based_occupancy_grid_map generates unnatural UNKNOWN cells in 2D manner: + +- all the cells behind pedestrians are UNKNOWN +- no ground point clouds are generated + +Also many users do not set traffic light information frequently although it is very critical for intersection_occlusion (and in real traffic environment too). + +For these reasons, `occlusion.enable` is false by default. + +#### on real vehicle / in end-to-end simulator + +On real vehicle or in end-to-end simulator like [AWSIM](https://tier4.github.io/AWSIM/) the following pointcloud_based_occupancy_grid_map configuration is highly recommended: + +```yaml +scan_origin_frame: "velodyne_top" + +grid_map_type: "OccupancyGridMapProjectiveBlindSpot" +OccupancyGridMapProjectiveBlindSpot: + projection_dz_threshold: 0.01 # [m] for avoiding null division + obstacle_separation_threshold: 1.0 # [m] fill the interval between obstacles with unknown for this length +``` + +You should set the top lidar link as the `scan_origin_frame`. In the example it is `velodyne_top`. The method `OccupancyGridMapProjectiveBlindSpot` estimates the FOV by running projective ray-tracing from `scan_origin` to obstacle or up to the ground and filling the cells on the "shadow" of the object as UNKNOWN. -### Flowchart +## Flowchart WIP @@ -305,9 +578,9 @@ stop ### Role -When an ego vehicle enters a public road from a private road (e.g. a parking lot), it needs to face and stop before entering the public road to make sure it is safe. +When an ego enters a public road from a private road (e.g. a parking lot), it needs to face and stop before entering the public road to make sure it is safe. -This module is activated when there is an intersection at the private area from which the vehicle enters the public road. The stop line is generated both when the goal is in the intersection lane and when the path goes beyond the intersection lane. The basic behavior is the same as the intersection module, but the ego vehicle must stop once at the stop line. +This module is activated when there is an intersection at the private area from which the vehicle enters the public road. The stop line is generated both when the goal is in the intersection lane and when the path goes beyond the intersection lane. The basic behavior is the same as the intersection module, but ego must stop once at the stop line. ![merge-from-private](docs/merge_from_private.png) @@ -326,4 +599,4 @@ This module is activated when the following conditions are met: ### Known Issue -If ego vehicle go over the stop line for a certain distance, then ego vehicle will not transit from STOP. +If ego go over the stop line for a certain distance, then it will not transit from STOP. diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 22f68733a3bc2..1e6ce843de528 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -2,42 +2,47 @@ ros__parameters: intersection: common: - attention_area_margin: 0.75 # [m] - attention_area_length: 200.0 # [m] - attention_area_angle_threshold: 0.785 # [rad] - stop_line_margin: 3.0 - intersection_velocity: 2.778 # 2.778m/s = 10.0km/h - intersection_max_accel: 0.5 # m/ss - stop_overshoot_margin: 0.5 # [m] overshoot margin for stuck, collision detection + attention_area_length: 200.0 + attention_area_margin: 0.75 + attention_area_angle_threshold: 0.785 use_intersection_area: false - path_interpolation_ds: 0.1 # [m] - consider_wrong_direction_vehicle: false + default_stopline_margin: 3.0 + stopline_overshoot_margin: 0.5 + path_interpolation_ds: 0.1 max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + stuck_vehicle: turn_direction: left: true right: true straight: true - use_stuck_stopline: true # stopline generated before the first conflicting area - stuck_vehicle_detect_dist: 5.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - # enable_front_car_decel_prediction: false # By default this feature is disabled - # assumed_front_car_decel: 1.0 # [m/ss] the expected deceleration of front car when front car as well as ego are turning - timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area - enable_private_area_stuck_disregard: false #In the intersections which labeled as "private area", the obstacle vehicles judged as "stuck" are neglected if this param is set as true. - yield_stuck: - turn_direction: - left: true - right: false - straight: false - distance_thr: 1.0 # [m] + use_stuck_stopline: true + stuck_vehicle_detect_dist: 5.0 + stuck_vehicle_velocity_threshold: 0.833 + # enable_front_car_decel_prediction: false + # assumed_front_car_decel: 1.0 + timeout_private_area: 3.0 + enable_private_area_stuck_disregard: false + + yield_stuck: + turn_direction: + left: true + right: false + straight: false + distance_threshold: 1.0 collision_detection: - state_transit_margin_time: 0.5 + consider_wrong_direction_vehicle: false + collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - minimum_ego_predicted_velocity: 1.388 # [m/s] + keep_detection_velocity_threshold: 0.833 + velocity_profile: + use_upstream: true + minimum_upstream_velocity: 0.01 + default_velocity: 2.778 + minimum_default_velocity: 1.388 fully_prioritized: collision_start_margin_time: 2.0 collision_end_margin_time: 0.0 @@ -45,52 +50,46 @@ collision_start_margin_time: 3.0 collision_end_margin_time: 2.0 not_prioritized: - collision_start_margin_time: 3.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object - collision_end_margin_time: 2.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object - keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr - use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module - minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity + collision_start_margin_time: 3.0 + collision_end_margin_time: 2.0 yield_on_green_traffic_light: distance_to_assigned_lanelet_start: 10.0 duration: 3.0 - object_dist_to_stopline: 10.0 # [m] + object_dist_to_stopline: 10.0 ignore_on_amber_traffic_light: - object_expected_deceleration: 2.0 # [m/ss] + object_expected_deceleration: 2.0 ignore_on_red_traffic_light: object_margin_to_path: 2.0 occlusion: enable: false - occlusion_attention_area_length: 70.0 # [m] - enable_creeping: false # flag to use the creep velocity when reaching occlusion limit stop line - occlusion_creep_velocity: 0.8333 # the creep velocity to occlusion limit stop line - peeking_offset: -0.5 # [m] offset for peeking into detection area + occlusion_attention_area_length: 70.0 free_space_max: 43 occupied_min: 58 - do_dp: true - before_creep_stop_time: 0.1 # [s] - min_vehicle_brake_for_rss: -2.5 # [m/s^2] - max_vehicle_velocity_for_rss: 16.66 # [m/s] == 60kmph - denoise_kernel: 1.0 # [m] - possible_object_bbox: [1.5, 2.5] # [m x m] - ignore_parked_vehicle_speed_threshold: 0.8333 # == 3.0km/h - stop_release_margin_time: 1.5 # [s] - temporal_stop_before_attention_area: false - absence_traffic_light: - creep_velocity: 1.388 # [m/s] - maximum_peeking_distance: 6.0 # [m] + denoise_kernel: 1.0 attention_lane_crop_curvature_threshold: 0.25 attention_lane_curvature_calculation_ds: 0.5 + creep_during_peeking: + enable: false + creep_velocity: 0.8333 + peeking_offset: -0.5 + occlusion_required_clearance_distance: 55 + possible_object_bbox: [1.5, 2.5] + ignore_parked_vehicle_speed_threshold: 0.8333 + occlusion_detection_hold_time: 1.5 + temporal_stop_time_before_peeking: 0.1 + temporal_stop_before_attention_area: false + creep_velocity_without_traffic_light: 1.388 static_occlusion_with_traffic_light_timeout: 0.5 debug: ttc: [0] enable_rtc: - intersection: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval + intersection: false intersection_to_occlusion: false merge_from_private: - stop_line_margin: 3.0 + stopline_margin: 3.0 stop_duration_sec: 1.0 stop_distance_threshold: 1.0 diff --git a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg index ec5878a0d828e..028ec8b32836c 100644 --- a/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/data-structure.drawio.svg @@ -5,31 +5,31 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2382px" - height="2070px" - viewBox="-0.5 -0.5 2382 2070" - content="<mxfile host="Electron" modified="2023-11-12T05:07:41.048Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="NhhSyCkZKf7kn4cJ36_Z" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + width="2515px" + height="2295px" + viewBox="-0.5 -0.5 2515 2295" + content="<mxfile host="Electron" modified="2023-11-14T02:56:49.728Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="TBnrjbXd8NQuLxVvVAEO" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - + - - - - - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + - + - + - - - + + + - - - - + + + + - - + + - - + + - - + + - - + + - - + + - - + + - - - - - - - - - - - + transform="translate(292.95,0)scale(-1,1)translate(-292.95,0)rotate(90,292.95,832.55)" + pointer-events="none" + /> + + + + + + + + + + +
@@ -285,18 +285,18 @@
- closest_idx + closest_idx - - - + + +
@@ -307,21 +307,21 @@
- stuck_stop_line + stuck_stop_line - - - - - - + + + + + +
@@ -332,18 +332,18 @@
- default_stop_line + default_stop_line - - - + + +
@@ -354,18 +354,18 @@
- first_attention_stop_line + first_attention_stop_line - - - + + +
@@ -380,25 +380,25 @@
- occlusion_peeking... + occlusion_peeking... - - - - - - - - - - - - - - - + + + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + - - + + - + - - - + + + - - - + + + - - + + - - + + - - + + - - + + - - + + - - + + - +
@@ -632,18 +640,18 @@
- next + next - - - + + +
@@ -656,18 +664,18 @@
- prev + prev - - - + + +
@@ -680,18 +688,18 @@
- ego_or_entry2exit + ego_or_entry2exit - - - + + +
@@ -704,15 +712,15 @@
- entry2ego + entry2ego - - + + -
+
@@ -722,7 +730,7 @@
- IntersectionStopLines + IntersectionStopLines @@ -730,7 +738,7 @@
@@ -741,28 +749,64 @@
- PathLanelets + PathLanelets + + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... - - - - + + + + + + + + + + + + - - - - - + - - + + - - - + - + - + + + + + + - - - - + + + + + + + + + + + + + + + + + + + + + + - - + + + + + + + + + + + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + +
- - - occlusion_wo_tl -
- _pass_judge_line -
- + + object
- occlusion_wo_tl... + object - - - +
- - closest_idx + + attention_lanelet
- closest_idx + attention_lanelet - - - - - - +
- - stuck_stop_line + + + stopline, +
+ dist_to_stopline +
+
- stuck_stop_line + stopline,... - - - + + + + + + +
- - default_stop_line + + object
- default_stop_line + object - - - +
- - first_attention_stop_line + + attention_lanelet
- first_attention_stop_line + attention_lanelet - - - +
- + - occlusion_peeking + stopline, +
+ dist_to_stopline
- _stop_line
- occlusion_peeking... + stopline,... - - - - - + + + + + +
-
-
- - - occlusion_wo_tl -
- _pass_judge_line -
- - +
+
+ + TargetObject +
- occlusion_wo_tl... + TargetObject - - - - - - - - - diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg index 57552f586e63b..94b63a97608ef 100644 --- a/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/intersection-attention.drawio.svg @@ -6,28 +6,28 @@ xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="3707px" - height="2195px" - viewBox="-0.5 -0.5 3707 2195" - content="<mxfile host="Electron" modified="2023-10-03T05:15:42.124Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="WBrLRHcQvF5FOJ4jcZAE" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + height="3570px" + viewBox="-0.5 -0.5 3707 3570" + content="<mxfile host="Electron" modified="2023-11-15T07:03:18.394Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="8yBIGjeANrMFIPyocWYk" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - + + - - - - + + + + - + - - + + - - - - - + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + - + - + - + - + - + - + - - - - - + + + + + - - - - - + + + + + - + + + + - - - - + - +
@@ -306,7 +314,7 @@

- (Left-hand traffic) + Left-hand traffic, RightOfWay is set on the map

@@ -337,22 +345,22 @@
- Urban crossroads with traffic light... + Urban crossroads with traffic light... - - - - - - - - - - - - + + + + + + + + + + + - + - + - + - - - - - + + + + + - + - - - + + + - - + + - - - - + + + + - - - + + + - - + + - + - - - + + + - - - + + + - - + + - - - - - - - + + + + + + + - +
- ego lane + ego lane - - - + + +
- yield lane + yield lane - - - + + +
- attention lane + attention lane - - + +
@@ -639,60 +647,66 @@
- conflicting lanes + conflicting lanes - - - - - - - - + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + - + - - - - + + + + - - - - - - + + + + + + - - - - - - + + + + + + - - - - - - - + + + + + + +
@@ -755,7 +769,7 @@

- (Right-hand traffic) + Right-hand traffic, RightOfWay is set on the map

@@ -786,16 +800,16 @@
- Urban crossroads with traffic light... + Urban crossroads with traffic light... - +
- ego lane + ego lane @@ -816,7 +830,7 @@
@@ -827,16 +841,16 @@
- conflicting lanes + conflicting lanes - +
- yield lane + yield lane - +
- attention lane + attention lane - - - - - - - + + + + + + + - - + + - - - - - - + + + + + + - - - - - + + + + + - - + + - - - + + + - + - + - - - - + + + + - - + +
@@ -1039,22 +1053,22 @@ T-shape junction w/o traffic light

- (Left-hand traffic) + Left-hand traffic, RightOfWay is not set on the map
- T-shape junction w/o traffic light... + T-shape junction w/o traffic light... - +
- ego lane + ego lane - - - + + +
- attention area + attention area - - - - - - + + + + + +
@@ -1111,30 +1125,31 @@ T-shape junction w/o traffic light

- (Right-hand traffic) + Right-hand traffic, RightOfWay is not set on the map +
- T-shape junction w/o traffic light... + T-shape junction w/o traffic light... - - - - - + + + + + - - - + + + - - + + - - - - + + + + - +
- ego lane + ego lane - - - + + +
- attention area + attention area - - - - + + + + + + + + + + + + + + + + + + + - - + + - - + + - - + + - - + + - - + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + +
+
+
+ attention area +
+
+
+ + attention area + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - + - - + + + + + + + + + + + + + + - - + + + + + + + + + + + + - - + + + + + +
+
+
+ + Urban crossroads with traffic light +
+
+ + Left-hand traffic, RightOfWay is not on the map +
+ + + + +
+ + +
+
+
+ + Urban crossroads with traffic light... + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + +
+
+
+ + conflicting lanes + +
+
+
+ + conflicting lanes + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + Urban crossroads with traffic light +
+
+ + Right-hand traffic, RightOfWay is not set on the map +
+ + + + +
+ + +
+
+
+ + Urban crossroads with traffic light... + + + + + + +
+
+
+ + ego lane + +
+
+
+ + ego lane + + + + + +
+
+
+ + conflicting lanes + +
+
+
+ + conflicting lanes + + + + + + +
+
+
+ attention lane +
+
+
+ + attention lane + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- attention area + attention area - - - + + +
- attention area + attention area - - + + diff --git a/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg b/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg new file mode 100644 index 0000000000000..67e4479f70d57 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/intersection-stoplines.drawio.svg @@ -0,0 +1,710 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + +
+
+
+ + closest_idx + +
+
+
+ + closest_idx + + + + + + + + + + + +
+
+
+ + stuck_stop_line + +
+
+
+ + stuck_stop_line + + + + + + + + +
+
+
+ + default_stop_line + +
+
+
+ + default_stop_line + + + + + + + + +
+
+
+ + first_attention_stop_line + +
+
+
+ + first_attention_stop_line + + + + + + + + +
+
+
+ + + occlusion_peeking +
+ _stop_line + + +
+
+
+ + occlusion_peeking... + + + + + + + + + + +
+
+
+ + + occlusion_wo_tl +
+ _pass_judge_line +
+ + +
+
+
+ + occlusion_wo_tl... + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg new file mode 100644 index 0000000000000..4edce74d53a7c --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-with-tl.drawio.svg @@ -0,0 +1,2002 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + ego + +
+
+
+ + ego + + + + + + + + + +
+
+
+ + nearest occlusion cell + +
+
+
+ + nearest occlusion... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + dynamically occluded by detected object +
+ + +
+
+
+ + dynamically occluded by detected object + + + + + +
+
+
+ + + statically occluded because no detected object is blocking +
+ + +
+
+
+ + statically occluded because no detected object is blocking + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg index 2fc22c8a4a401..697c0c634daf0 100644 --- a/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/occlusion-without-tl.drawio.svg @@ -5,32 +5,32 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="1653px" - height="522px" - viewBox="-0.5 -0.5 1653 522" - content="<mxfile host="Electron" modified="2023-10-03T05:36:02.775Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="BwI40dFIjy7ASgJYnXFl" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">7V1Zl5tGFv41/ShO7ctjr845k8x4xplMnBcfWqLVJLRQEO3uzq+fArEVVUKFGpAcSz62RQGFqO+7t+5WcIGvn14/JP768ad4EUQXCCxeL/DNBUJQYsI8pL5ljW/bRgEo3bYsk3BRHFg3fAr/CopGULQ+h4tgox2YxnGUhmu9cR6vVsE81dr8JIlf9MMe4ki/6tpfBkbDp7kfla3lHWTt/wsX6WNxI4jX7T8E4fKxvDZkcrvnyS8PLu5l8+gv4pdGE769wNdJHKfbb0+v10GUDWE5Mr9cfyBvf/538fPlYn13d33z+bcf7mbbzu76nFLdWhKs0oO7fvpP7H+4//T5H1/YyyL8mTCebmbF6Hz1o+dixIp7Td/KIQxWi8sMCbW1ileq8eoxfYrUFlRfN2kS/1ENLFYtjr+1uKdgoeFX/PIPQfwUpMmbOuClxowWODw24CrbkiDy0/CrzgW/IM+y6q66wsc4VD8PgYLsM8gxLroqqD6jnBK9n038nMyD4tTmSBu9MciI0RvVe0v9ZBmkRm9qrP23xmHr7IBN508HQhgXw80+1Zdtt+VWY4jrppwvPbiDz9zZyR0qW/304I5E7d4QByNxx3IxKjU9Mg53yJk7labYBcAh3OEMtbnDxFjcMS9GhfAKrTkqfejg9Fn4m8dgoe2+jqM4yTvDV3fZn2+YZYKYOuVwDWX2ZnB2B8uGIgB0YMAyiZ/X/QD7GiRp8KoNc2GZ+vdlt8AOZUOlEupJySjEmXqF7amZC7VXciYpIFIWirBJBAg9TKvTkTB5QTK7FlV9YMZ3U0Ib+d7DzPYPczZmobK4f/Tvg+hjvAnTMF6pXfdxmsZPSmbKAy6jcJntSON1Sxof/XXW2dPrMvNEvHt/E849hVl6Ha/mt1GkvIUgF0vVdLla5lcGnmCIC4g5ARBL9VcdkQl9tR9CoEaGAi45Z5RItd9P5qXQA0+NGr56CKOoKej5H9WexKlf3MhMsb2tEwqVEq/9eZhm2FJg1Q7d5G2zbSerMNMowTnVWIW5hzBGlGFMuQDYJBUU0KQR5HIs3vD9vFHSuVrkKjcbupfHMA0+qeHM9r4oGugUcYDp6CBB1EbJAKkhs9zACAtqYsTAWBCJ6SGaSVeQlCJYhAqg1lGjYTcTSl8QTAQTGHDBEdawQwR7vBM8JCcFT77TANKRNe2dEjzdUCImUMFrmP5adJp9/5yrVlps3bwWV8g33sqNlRqDX/MD1URYNhRnkqqhPjnfqs9e3IXZcNX7PgZJqAY2SIrGnjTZ2ioOsnIi9hyTTfuihP7NzTZwNfM6LwKxsCmziYy/0mXtYv/8Ofla2fMTiUJF6xalexG6J3e3cDpwdz/J+bdDcmXwaUY0xIeRHMKuqwjWeZGBHOnOG+XW+xzVty6hcpxb5pG/Ueb6yNMLOGR6gf3kcHDJ49+m5BElXMqdIoICACWGUg9lI6CsXEQJ5kJySaVoxZdcZY/qvC9NmlLChbpKPcEIMPEU42BgHSm+wDHSHYsZ5YQZBimEDFssUjXoHYR4l1GKHablwT0KKLKOHuLaVVBHbj+dmucwp7CixX7H4rhQjKTDm5raeWwPtYt6YrJXzZb8PBE1yzxCiAQIMsShEKQV0qEeaH4OtOKzWCSiCEMOpeCtpOSea4ysYjE6XRWLiKFiBVSDhQFiGAuCTH1LgWwE4bjkxMIcCCydDC/9DpnZv33Ulg4cD6zo6hxTQhJmxnv1IXpMCXN7TPd4UdsybjzlBO4cERwPJgq6YEIiw6mWWWiKPha26X2s2B92SH2NZGYdB6emvFQ40GoyOx4QDsmxyR3lw+KwoxhmJcR7HWXsGoctDzwRCw4LpBxlJpX5pnwyKHWbIXNgqQCAIYzUlNfW2q4WnK6NmIdZLQNMutV2DGayOaT1dgVeTzhO9M3Qn5wW/anuskCmW7fgwKAsAa1+YXe/A8VhSWvi59arjltR6ZCTnV6kPI40qUKYdMuV2jgwI+cuM/ikRIHgFmXlQKLQigp0O5Bja//TjYkyUVbI1g57OeYNBjBkoYCR1znEQPznw+Xrxy+Es+XV6re3j/jzL//+3aW4vu1Eb5SzvfHSxF9t1nGytb+9uZ98wbqU58osSG6/BplOs9rxAYUE2HUAyqI9DR+/ZxDAL7bmwSqX8Hb49SH/9JB696hq5bPpkSxhiX8DC9aIvh9qe0GqLeDKInXZq3sNcfbnc7ZeJB+y2SZfL3OpDsBg/ZoPWLlffVsW/0fl8UN2NK/gqg9UwOXEMM5dBA/+c7S9vmJAeWNJuT8KlSdYtKrh215Cv2x98J4DVfN93bbTr4X7/dooY/WVP/9jmZ/WIOhd/rGJxt1dIRrZTyuWM2FqUvyGy6vtcQ2h61ek1J/1UDBPnw8QNpVcVqtkqUUSY9WIUgc1906jxRWV7JPjeog7oBtE1Apk1zy212YpdcSJ2Cxq1sQtNTqjuAxbH1IVbqxkIBNXhVNb4LtLCw+rJuP5PHreZFPo96fiell4HZYclYYlx4AwlZy0RMZHi/RRh8D4WcU1BPB0VBwq4/X1UhXUokmvZZ0GOfHEKs6hyOJohSnGWDMpmkU+prFCAWkuXYDYEsAnQHqizpThtjM9mJij7y1vyq7Fbb6sbcS8aUVYd+dOGAsnsAX+4+VJ0ch5UhssQ+dJD4BF6tKNsNiHC5o0H4dGTozaYJlmPUsPsJxlBnPLwsLxsOmXK+1dpV9hc1KJ0oMq+0uw9xpbpRramzdCp5U3Iq2cJlE2gI2svYPlvFVgQ7v7HbuE2CFZ+k6noT/rgYdYK5sj+DjcPzwL5C4B2FUCTqtwALWYKocRANwWrOPy3yGZOdgqrQMkoZf+n0KPnwg5CUEaixR9PIZrGjF5IDu5/pQifd0Txm7JzP5Z/dbdAOvd7JxTsH666Hc2dLnloUoKrDlIh3DBiDlIACgIHmwCCwC8ubq+GC4H2faV3pmT7O8f4bZ/xMA+G1xKU4pxq6RrsGz02GvGrRFMCxLVcS7RyE5sTkVhAv0ZLzCv+96t2dzt2X79DjefW/nj4F1/b9UMQyU9GBFG5NQSLB2tpMGKt4P5VuI9f4tCpRoSvD+mcr9VIj/eVw1V9upfz2leSFCESbcaBFKTCz6gCC06kDLNwxt4B+y222AYKhBb2fkZpdySurJpogEWAlpBdND5e8WmgZ6jBLX1wEMUx8k68lde9k9qsxOED7pmbYv9QPnoiBrPt7OlIsulYpPgCXvYc2epVBjmTyQ0pNLy/J4ppRI6LM89i+VOsQSmWJqATiuW6CyW/cQSc4tYmlWc04rlEPnf71YsKYUXbbE0AZ1WLB0StWexbPohHJ2gWDp4nmex7OFaHl0sHZLBZ7Fs1ulKmxHrKJYDpPTtKDrkN89iuVMsS5Oxv1iOBaj1wW9/mzUsD2GyyU7y01Sh2SjSrlembBe3gPNqlmlKvXm2StJQa8R8rMOIK1rsguDgzR2r3LtP2dKQ5d6dGuNEEjHZGhQKTUoh6rWC5O96Gwt1fBvLUIkXl7LkIyfuGk905gBdaIVCAIqLfWVyqcsznLuour8C6ORWJkhoELWt0Xq9kmNvbyPX+xAHq367MsH6BEfdaKtmJIOtC3bPKLtwqHse/0Uf5RLO2oZjpg3Ha596qsc/kpGeI9H5+McKsmkf/1jS7kQE23g1x6Fv2cmMHgJolnISCHAgdX8BE09SUj+1bLTaPrvv1yM5rBD0oyiIYuWvZS7fuqHotX2NGWB/Zf5rUL5t0vrCph1zFXO1cbs1XI81E20L15I/FGg3G3uoBrVZv4tyC3T9Xk98+38=</diagram></mxfile>" + width="1357px" + height="600px" + viewBox="-0.5 -0.5 1357 600" + content="<mxfile host="Electron" modified="2023-11-14T20:57:10.757Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="Xql1fZtAubmV39K-nFIb" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - + + + + - - - - - + + + + + - - + + - - - + + + - + - + - +
-
+
- - - default stop + + 1.stop at the default_ + + + + stopline + + + +
- line -
- default stop... + 1.stop at the defa... - - - + + +
- occlusion + occlusion
- occlusion + occlusion - - - - - - + + + + - + - - - + + + - - - + + + - - - + + + - - - + + + - - - + + + - +
-
+
- - - - first attention -
- stop line - -
- - +
+ + 2. stop at the first_attention_stopline + +
- first attentio... + 2. stop at the first_... - - - - - + + + + + - + + + + + + +
+
+
+
+ + 3. creep up to occlusion_wo_tl_ + + + pass_judge_line + +
+
+
+
+ + 3. creep up to occlusion_wo_... + + + + diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png b/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png new file mode 100644 index 0000000000000..133b6947cbe42 Binary files /dev/null and b/planning/behavior_velocity_intersection_module/docs/occlusion-wo-tl-creeping.png differ diff --git a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg index fc09a4212070a..ff1f9843c2b1d 100644 --- a/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/occlusion_grid.drawio.svg @@ -5,30 +5,30 @@ xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" - width="2502px" + width="2523px" height="1392px" - viewBox="-0.5 -0.5 2502 1392" - content="<mxfile host="Electron" modified="2023-09-29T04:34:40.611Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="5DwL00YCo2z-J-veTRkD" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" + viewBox="-0.5 -0.5 2523 1392" + content="<mxfile host="Electron" modified="2023-11-13T12:12:55.311Z" agent="5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/20.3.0 Chrome/104.0.5112.114 Electron/20.1.3 Safari/537.36" etag="LhhfO7V8Yd9xK0iSo8Ik" version="20.3.0" type="device"><diagram name="intersection" id="0L5whF3ImEvTl2DSWTjR">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</diagram></mxfile>" > - - - - - + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + - + - + - + - + - + - - - - - + + + + + - - - - - + + + + + - + - - - - + + + + - +
- default stop line + default_stopline
- default stop line + default_stopline - - - + + + -
+
- occlusion peeking line + occlusion_peeking_stopline
- occlusion peeking line + occlusion_peeking_stopline - - - + + + - + - - - + + + - - + +
- ego + ego - - - + + +
- occlusion attent... + occlusion attent... - - + + - +
- stopped vehicle on attentio... + stopped vehicle on attentio... - - + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- + The cells behind blocking vehicles
are not marked as occluded -
+
- The cells behind blocking vehicles... + The cells behind blocking vehicles... - - - + + + -
+
- mark the cells which is unknown and... + mark the cells which is unknown and... - - - + + + - - - + + + - - - - + + + + - - - - + + + +
@@ -1350,17 +1350,17 @@
- 13 + 13 - - + +
@@ -1369,17 +1369,17 @@
- 12 + 12 - - + +
@@ -1388,17 +1388,17 @@
- 12 + 12 - - + +
@@ -1407,32 +1407,32 @@
- 11 + 11 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -1441,17 +1441,17 @@
- 12 + 12 - - + +
@@ -1460,17 +1460,17 @@
- 11 + 11 - - + +
@@ -1479,17 +1479,17 @@
- 11 + 11 - - + +
@@ -1498,32 +1498,32 @@
- 10 + 10 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -1532,17 +1532,17 @@
- 11 + 11 - - + +
@@ -1551,17 +1551,17 @@
- 10 + 10 - - + +
@@ -1570,17 +1570,17 @@
- 10 + 10 - - + +
@@ -1589,32 +1589,32 @@
- 9 + 9 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -1623,17 +1623,17 @@
- 10 + 10 - - + +
@@ -1642,17 +1642,17 @@
- 9 + 9 - - + +
@@ -1661,16 +1661,16 @@
- 9 + 9 - +
@@ -1679,32 +1679,32 @@
- 8 + 8 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -1713,17 +1713,17 @@
- 9 + 9 - - + +
@@ -1732,16 +1732,16 @@
- 8 + 8 - +
@@ -1750,16 +1750,16 @@
- 8 + 8 - +
@@ -1768,33 +1768,33 @@
- 7 + 7 - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + +
@@ -1803,17 +1803,17 @@
- 8 + 8 - - + +
@@ -1822,16 +1822,16 @@
- 7 + 7 - +
@@ -1840,16 +1840,16 @@
- 7 + 7 - +
@@ -1858,33 +1858,33 @@
- 6 + 6 - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + +
@@ -1893,16 +1893,16 @@
- 7 + 7 - +
@@ -1911,16 +1911,16 @@
- 6 + 6 - +
@@ -1929,16 +1929,16 @@
- 6 + 6 - +
@@ -1947,33 +1947,33 @@
- 5 + 5 - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + +
@@ -1982,16 +1982,16 @@
- 6 + 6 - +
@@ -2000,16 +2000,16 @@
- 5 + 5 - +
@@ -2018,16 +2018,16 @@
- 5 + 5 - +
@@ -2036,33 +2036,33 @@
- 4 + 4 - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + +
@@ -2071,16 +2071,16 @@
- 5 + 5 - +
@@ -2089,16 +2089,16 @@
- 4 + 4 - +
@@ -2107,16 +2107,16 @@
- 4 + 4 - +
@@ -2125,33 +2125,33 @@
- 3 + 3 - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + +
@@ -2164,16 +2164,16 @@
- 4... + 4... - +
@@ -2182,16 +2182,16 @@
- 3 + 3 - +
@@ -2200,16 +2200,16 @@
- 3 + 3 - +
@@ -2218,32 +2218,32 @@
- 2 + 2 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -2252,16 +2252,16 @@
- 3 + 3 - +
@@ -2270,16 +2270,16 @@
- 2 + 2 - +
@@ -2288,16 +2288,16 @@
- 2 + 2 - +
@@ -2306,32 +2306,32 @@
- 1 + 1 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -2340,16 +2340,16 @@
- 2 + 2 - +
@@ -2358,16 +2358,16 @@
- 1 + 1 - +
@@ -2376,16 +2376,16 @@
- 1 + 1 - +
@@ -2394,32 +2394,32 @@
- 0 + 0 - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + +
@@ -2428,16 +2428,16 @@
- 1 + 1 - +
@@ -2446,34 +2446,34 @@
- 0 + 0 - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + +
@@ -2482,115 +2482,115 @@
- 0 + 0 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
- nearest occlusion... + nearest occlusion... diff --git a/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg b/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg new file mode 100644 index 0000000000000..6a268a9b83c6e --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/signal-phase-group.drawio.svg @@ -0,0 +1,547 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + Urban crossroads with traffic light +
+
+ (Left-hand traffic) +
+
+ inphase signal group + of lane + L1 +
+ + +
+
+
+ + Urban crossroads with traffic light... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + Urban crossroads with traffic light +
+
+ (Right-hand traffic) +
+
+ + + antiphase signal group + of lane + L1 + + +
+ + +
+
+
+ + Urban crossroads with traffic light... + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ L1 +
+
+
+ + L1 + + + + + +
+
+
+ L1 +
+
+
+ + L1 + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png b/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png new file mode 100644 index 0000000000000..9bb9188db745f Binary files /dev/null and b/planning/behavior_velocity_intersection_module/docs/static-occlusion-timeout.png differ diff --git a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg index c29cb7ade21cd..4cae662d8c764 100644 --- a/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg +++ b/planning/behavior_velocity_intersection_module/docs/stuck-vehicle.drawio.svg @@ -539,13 +539,13 @@
- stop_line_margin + stopline_margin
- stop_line_margin + stopline_margin diff --git a/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg b/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg new file mode 100644 index 0000000000000..2e0581e8da378 --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/ugly-intersection.drawio.svg @@ -0,0 +1,326 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + + complex/bad intersection map +
+ + +
+
+
+ + complex/bad intersection map + + + + + +
+
+
+ + (1) ego lane is overlapped with adjacnet lane. +
+ This causes unnecessary attention lanes. +
+ +
+
+
+ + (1) ego lane is overlapped with adjacnet lane.... + + + + + + + +
+
+
+ + (2) ego lane is overlapped with unrelated lane +
+ +
+
+
+ +  (2) ego lane is overlapped with unrelated lane + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 5de74aeb82d86..789708abe98f8 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -43,25 +43,20 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) { const std::string ns(getModuleName()); auto & ip = intersection_param_; - ip.common.attention_area_margin = - getOrDeclareParameter(node, ns + ".common.attention_area_margin"); ip.common.attention_area_length = getOrDeclareParameter(node, ns + ".common.attention_area_length"); - ip.common.attention_area_angle_thr = + ip.common.attention_area_margin = + getOrDeclareParameter(node, ns + ".common.attention_area_margin"); + ip.common.attention_area_angle_threshold = getOrDeclareParameter(node, ns + ".common.attention_area_angle_threshold"); - ip.common.stop_line_margin = getOrDeclareParameter(node, ns + ".common.stop_line_margin"); - ip.common.intersection_velocity = - getOrDeclareParameter(node, ns + ".common.intersection_velocity"); - ip.common.intersection_max_acc = - getOrDeclareParameter(node, ns + ".common.intersection_max_accel"); - ip.common.stop_overshoot_margin = - getOrDeclareParameter(node, ns + ".common.stop_overshoot_margin"); ip.common.use_intersection_area = getOrDeclareParameter(node, ns + ".common.use_intersection_area"); + ip.common.default_stopline_margin = + getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.stopline_overshoot_margin = + getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); ip.common.path_interpolation_ds = getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); - ip.common.consider_wrong_direction_vehicle = - getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = @@ -77,33 +72,38 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".stuck_vehicle.use_stuck_stopline"); ip.stuck_vehicle.stuck_vehicle_detect_dist = getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_detect_dist"); - ip.stuck_vehicle.stuck_vehicle_vel_thr = - getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_vel_thr"); - /* - ip.stuck_vehicle.assumed_front_car_decel = - getOrDeclareParameter(node, ns + ".stuck_vehicle.assumed_front_car_decel"); - ip.stuck_vehicle.enable_front_car_decel_prediction = - getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_front_car_decel_prediction"); - */ + ip.stuck_vehicle.stuck_vehicle_velocity_threshold = + getOrDeclareParameter(node, ns + ".stuck_vehicle.stuck_vehicle_velocity_threshold"); ip.stuck_vehicle.timeout_private_area = getOrDeclareParameter(node, ns + ".stuck_vehicle.timeout_private_area"); ip.stuck_vehicle.enable_private_area_stuck_disregard = getOrDeclareParameter(node, ns + ".stuck_vehicle.enable_private_area_stuck_disregard"); - ip.stuck_vehicle.yield_stuck_turn_direction.left = - getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.left"); - ip.stuck_vehicle.yield_stuck_turn_direction.right = - getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.right"); - ip.stuck_vehicle.yield_stuck_turn_direction.straight = - getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.turn_direction.straight"); - ip.stuck_vehicle.yield_stuck_distance_thr = - getOrDeclareParameter(node, ns + ".stuck_vehicle.yield_stuck.distance_thr"); + ip.yield_stuck.turn_direction.left = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.left"); + ip.yield_stuck.turn_direction.right = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.right"); + ip.yield_stuck.turn_direction.straight = + getOrDeclareParameter(node, ns + ".yield_stuck.turn_direction.straight"); + ip.yield_stuck.distance_threshold = + getOrDeclareParameter(node, ns + ".yield_stuck.distance_threshold"); + + ip.collision_detection.consider_wrong_direction_vehicle = + getOrDeclareParameter(node, ns + ".collision_detection.consider_wrong_direction_vehicle"); + ip.collision_detection.collision_detection_hold_time = + getOrDeclareParameter(node, ns + ".collision_detection.collision_detection_hold_time"); ip.collision_detection.min_predicted_path_confidence = getOrDeclareParameter(node, ns + ".collision_detection.min_predicted_path_confidence"); - ip.collision_detection.minimum_ego_predicted_velocity = - getOrDeclareParameter(node, ns + ".collision_detection.minimum_ego_predicted_velocity"); - ip.collision_detection.state_transit_margin_time = - getOrDeclareParameter(node, ns + ".collision_detection.state_transit_margin_time"); + ip.collision_detection.keep_detection_velocity_threshold = getOrDeclareParameter( + node, ns + ".collision_detection.keep_detection_velocity_threshold"); + ip.collision_detection.velocity_profile.use_upstream = + getOrDeclareParameter(node, ns + ".collision_detection.velocity_profile.use_upstream"); + ip.collision_detection.velocity_profile.minimum_upstream_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_upstream_velocity"); + ip.collision_detection.velocity_profile.default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.default_velocity"); + ip.collision_detection.velocity_profile.minimum_default_velocity = getOrDeclareParameter( + node, ns + ".collision_detection.velocity_profile.minimum_default_velocity"); ip.collision_detection.fully_prioritized.collision_start_margin_time = getOrDeclareParameter( node, ns + ".collision_detection.fully_prioritized.collision_start_margin_time"); @@ -121,12 +121,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) node, ns + ".collision_detection.not_prioritized.collision_start_margin_time"); ip.collision_detection.not_prioritized.collision_end_margin_time = getOrDeclareParameter( node, ns + ".collision_detection.not_prioritized.collision_end_margin_time"); - ip.collision_detection.keep_detection_vel_thr = - getOrDeclareParameter(node, ns + ".collision_detection.keep_detection_vel_thr"); - ip.collision_detection.use_upstream_velocity = - getOrDeclareParameter(node, ns + ".collision_detection.use_upstream_velocity"); - ip.collision_detection.minimum_upstream_velocity = - getOrDeclareParameter(node, ns + ".collision_detection.minimum_upstream_velocity"); ip.collision_detection.yield_on_green_traffic_light.distance_to_assigned_lanelet_start = getOrDeclareParameter( node, @@ -146,41 +140,35 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.occlusion.enable = getOrDeclareParameter(node, ns + ".occlusion.enable"); ip.occlusion.occlusion_attention_area_length = getOrDeclareParameter(node, ns + ".occlusion.occlusion_attention_area_length"); - ip.occlusion.enable_creeping = - getOrDeclareParameter(node, ns + ".occlusion.enable_creeping"); - ip.occlusion.occlusion_creep_velocity = - getOrDeclareParameter(node, ns + ".occlusion.occlusion_creep_velocity"); - ip.occlusion.peeking_offset = - getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); ip.occlusion.free_space_max = getOrDeclareParameter(node, ns + ".occlusion.free_space_max"); ip.occlusion.occupied_min = getOrDeclareParameter(node, ns + ".occlusion.occupied_min"); - ip.occlusion.do_dp = getOrDeclareParameter(node, ns + ".occlusion.do_dp"); - ip.occlusion.before_creep_stop_time = - getOrDeclareParameter(node, ns + ".occlusion.before_creep_stop_time"); - ip.occlusion.min_vehicle_brake_for_rss = - getOrDeclareParameter(node, ns + ".occlusion.min_vehicle_brake_for_rss"); - ip.occlusion.max_vehicle_velocity_for_rss = - getOrDeclareParameter(node, ns + ".occlusion.max_vehicle_velocity_for_rss"); ip.occlusion.denoise_kernel = getOrDeclareParameter(node, ns + ".occlusion.denoise_kernel"); + ip.occlusion.attention_lane_crop_curvature_threshold = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); + ip.occlusion.attention_lane_curvature_calculation_ds = + getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.creep_during_peeking.enable = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.enable"); + ip.occlusion.creep_during_peeking.creep_velocity = + getOrDeclareParameter(node, ns + ".occlusion.creep_during_peeking.creep_velocity"); + ip.occlusion.peeking_offset = + getOrDeclareParameter(node, ns + ".occlusion.peeking_offset"); ip.occlusion.possible_object_bbox = getOrDeclareParameter>(node, ns + ".occlusion.possible_object_bbox"); ip.occlusion.ignore_parked_vehicle_speed_threshold = getOrDeclareParameter(node, ns + ".occlusion.ignore_parked_vehicle_speed_threshold"); - ip.occlusion.stop_release_margin_time = - getOrDeclareParameter(node, ns + ".occlusion.stop_release_margin_time"); + ip.occlusion.occlusion_detection_hold_time = + getOrDeclareParameter(node, ns + ".occlusion.occlusion_detection_hold_time"); + ip.occlusion.temporal_stop_time_before_peeking = + getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_time_before_peeking"); ip.occlusion.temporal_stop_before_attention_area = getOrDeclareParameter(node, ns + ".occlusion.temporal_stop_before_attention_area"); - ip.occlusion.absence_traffic_light.creep_velocity = - getOrDeclareParameter(node, ns + ".occlusion.absence_traffic_light.creep_velocity"); - ip.occlusion.absence_traffic_light.maximum_peeking_distance = getOrDeclareParameter( - node, ns + ".occlusion.absence_traffic_light.maximum_peeking_distance"); - ip.occlusion.attention_lane_crop_curvature_threshold = - getOrDeclareParameter(node, ns + ".occlusion.attention_lane_crop_curvature_threshold"); - ip.occlusion.attention_lane_curvature_calculation_ds = - getOrDeclareParameter(node, ns + ".occlusion.attention_lane_curvature_calculation_ds"); + ip.occlusion.creep_velocity_without_traffic_light = + getOrDeclareParameter(node, ns + ".occlusion.creep_velocity_without_traffic_light"); ip.occlusion.static_occlusion_with_traffic_light_timeout = getOrDeclareParameter( node, ns + ".occlusion.static_occlusion_with_traffic_light_timeout"); + ip.debug.ttc = getOrDeclareParameter>(node, ns + ".debug.ttc"); } @@ -220,8 +208,8 @@ void IntersectionModuleManager::launchNewModules( if (const auto tl_reg_elems = ll.regulatoryElementsAs(); tl_reg_elems.size() != 0) { const auto tl_reg_elem = tl_reg_elems.front(); - const auto stop_line_opt = tl_reg_elem->stopLine(); - if (!!stop_line_opt) has_traffic_light = true; + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; } const auto new_module = std::make_shared( module_id, lane_id, planner_data_, intersection_param_, associative_ids, turn_direction, @@ -340,7 +328,7 @@ MergeFromPrivateModuleManager::MergeFromPrivateModuleManager(rclcpp::Node & node mp.stop_duration_sec = getOrDeclareParameter(node, ns + ".stop_duration_sec"); mp.attention_area_length = node.get_parameter("intersection.common.attention_area_length").as_double(); - mp.stop_line_margin = getOrDeclareParameter(node, ns + ".stop_line_margin"); + mp.stopline_margin = getOrDeclareParameter(node, ns + ".stopline_margin"); mp.path_interpolation_ds = node.get_parameter("intersection.common.path_interpolation_ds").as_double(); mp.stop_distance_threshold = getOrDeclareParameter(node, ns + ".stop_distance_threshold"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 476238f6b1e9e..0d483b501d1ee 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -95,7 +95,7 @@ static bool isTargetCollisionVehicleType( IntersectionModule::IntersectionModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & associative_ids, + const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) @@ -110,24 +110,26 @@ IntersectionModule::IntersectionModule( is_private_area_(is_private_area), occlusion_uuid_(tier4_autoware_utils::generateUUID()) { - velocity_factor_.init(VelocityFactor::INTERSECTION); + velocity_factor_.init(PlanningBehavior::INTERSECTION); planner_param_ = planner_param; { collision_state_machine_.setMarginTime( - planner_param_.collision_detection.state_transit_margin_time); + planner_param_.collision_detection.collision_detection_hold_time); } { - before_creep_state_machine_.setMarginTime(planner_param_.occlusion.before_creep_stop_time); + before_creep_state_machine_.setMarginTime( + planner_param_.occlusion.temporal_stop_time_before_peeking); before_creep_state_machine_.setState(StateMachine::State::STOP); } { - occlusion_stop_state_machine_.setMarginTime(planner_param_.occlusion.stop_release_margin_time); + occlusion_stop_state_machine_.setMarginTime( + planner_param_.occlusion.occlusion_detection_hold_time); occlusion_stop_state_machine_.setState(StateMachine::State::GO); } { temporal_stop_before_attention_state_machine_.setMarginTime( - planner_param_.occlusion.before_creep_stop_time); + planner_param_.occlusion.occlusion_detection_hold_time); temporal_stop_before_attention_state_machine_.setState(StateMachine::State::STOP); } { @@ -193,14 +195,14 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "StuckStop"); const auto closest_idx = result.closest_idx; - const auto stop_line_idx = result.stuck_stop_line_idx; + const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; - if (result.occlusion_stop_line_idx) { - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx.value(); + if (result.occlusion_stopline_idx) { + const auto occlusion_stopline_idx = result.occlusion_stopline_idx.value(); *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); } return; } @@ -213,9 +215,9 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "YieldStuckStop"); const auto closest_idx = result.closest_idx; - const auto stop_line_idx = result.stuck_stop_line_idx; + const auto stopline_idx = result.stuck_stopline_idx; *default_safety = false; - *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stop_line_idx); + *default_distance = motion_utils::calcSignedArcLength(path.points, closest_idx, stopline_idx); *occlusion_safety = true; return; } @@ -228,14 +230,14 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "NonOccludedCollisionStop"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.collision_stop_line_idx; + const auto collision_stopline_idx = result.collision_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); - const auto occlusion_stop_line = result.occlusion_stop_line_idx; + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); + const auto occlusion_stopline = result.occlusion_stopline_idx; *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline); return; } @@ -247,14 +249,14 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FirstWaitBeforeOcclusion"); const auto closest_idx = result.closest_idx; - const auto first_stop_line_idx = result.first_stop_line_idx; - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; + const auto first_stopline_idx = result.first_stopline_idx; + const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, first_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, first_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -266,14 +268,14 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "PeekingTowardOcclusion"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.collision_stop_line_idx; - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; + const auto collision_stopline_idx = result.collision_stopline_idx; + const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -285,10 +287,10 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedAbsenceTrafficLight"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.closest_idx; + const auto collision_stopline_idx = result.closest_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = 0; return; @@ -302,14 +304,14 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "OccludedCollisionStop"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.collision_stop_line_idx; - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; + const auto collision_stopline_idx = result.collision_stopline_idx; + const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = false; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = result.is_actually_occlusion_cleared; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -321,33 +323,33 @@ void prepareRTCByDecisionResult( { RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "Safe"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.collision_stop_line_idx; - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; + const auto collision_stopline_idx = result.collision_stopline_idx; + const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = true; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } template <> void prepareRTCByDecisionResult( - const IntersectionModule::TrafficLightArrowSolidOn & result, + const IntersectionModule::FullyPrioritized & result, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, bool * default_safety, double * default_distance, bool * occlusion_safety, double * occlusion_distance) { - RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "TrafficLightArrowSolidOn"); + RCLCPP_DEBUG(rclcpp::get_logger("prepareRTCByDecisionResult"), "FullyPrioritized"); const auto closest_idx = result.closest_idx; - const auto collision_stop_line_idx = result.collision_stop_line_idx; - const auto occlusion_stop_line_idx = result.occlusion_stop_line_idx; + const auto collision_stopline_idx = result.collision_stopline_idx; + const auto occlusion_stopline_idx = result.occlusion_stopline_idx; *default_safety = !result.collision_detected; *default_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, collision_stopline_idx); *occlusion_safety = true; *occlusion_distance = - motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stop_line_idx); + motion_utils::calcSignedArcLength(path.points, closest_idx, occlusion_stopline_idx); return; } @@ -411,34 +413,34 @@ void reactRTCApprovalByDecisionResult( const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stuck_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.stuck_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if ( - !rtc_occlusion_approved && decision_result.occlusion_stop_line_idx && + !rtc_occlusion_approved && decision_result.occlusion_stopline_idx && planner_param.occlusion.enable) { - const auto occlusion_stop_line_idx = decision_result.occlusion_stop_line_idx.value(); - planning_utils::setVelocityFromIndex(occlusion_stop_line_idx, 0.0, path); + const auto occlusion_stopline_idx = decision_result.occlusion_stopline_idx.value(); + planning_utils::setVelocityFromIndex(occlusion_stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(occlusion_stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(occlusion_stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(occlusion_stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, - path->points.at(occlusion_stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(occlusion_stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -459,18 +461,18 @@ void reactRTCApprovalByDecisionResult( const auto closest_idx = decision_result.closest_idx; if (!rtc_default_approved) { // use default_rtc uuid for stuck vehicle detection - const auto stop_line_idx = decision_result.stuck_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.stuck_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; stop_factor.stop_factor_points = planning_utils::toRosPoints(debug_data->conflicting_targets); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -489,31 +491,31 @@ void reactRTCApprovalByDecisionResult( "NonOccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.collision_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.collision_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.occlusion_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -532,39 +534,39 @@ void reactRTCApprovalByDecisionResult( "FirstWaitBeforeOcclusion, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.first_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.first_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_first_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - if (planner_param.occlusion.enable_creeping) { - const size_t occlusion_peeking_stop_line = decision_result.occlusion_stop_line_idx; + if (planner_param.occlusion.creep_during_peeking.enable) { + const size_t occlusion_peeking_stopline = decision_result.occlusion_stopline_idx; const size_t closest_idx = decision_result.closest_idx; - for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { + for (size_t i = closest_idx; i < occlusion_peeking_stopline; i++) { planning_utils::setVelocityFromIndex( - i, planner_param.occlusion.occlusion_creep_velocity, path); + i, planner_param.occlusion.creep_during_peeking.creep_velocity, path); } } - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.occlusion_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -585,43 +587,43 @@ void reactRTCApprovalByDecisionResult( // NOTE: creep_velocity should be inserted first at closest_idx if !rtc_default_approved if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const size_t occlusion_peeking_stop_line = + const size_t occlusion_peeking_stopline = decision_result.temporal_stop_before_attention_required - ? decision_result.first_attention_stop_line_idx - : decision_result.occlusion_stop_line_idx; - if (planner_param.occlusion.enable_creeping) { + ? decision_result.first_attention_stopline_idx + : decision_result.occlusion_stopline_idx; + if (planner_param.occlusion.creep_during_peeking.enable) { const size_t closest_idx = decision_result.closest_idx; - for (size_t i = closest_idx; i < occlusion_peeking_stop_line; i++) { + for (size_t i = closest_idx; i < occlusion_peeking_stopline; i++) { planning_utils::setVelocityFromIndex( - i, planner_param.occlusion.occlusion_creep_velocity, path); + i, planner_param.occlusion.creep_during_peeking.creep_velocity, path); } } - planning_utils::setVelocityFromIndex(occlusion_peeking_stop_line, 0.0, path); + planning_utils::setVelocityFromIndex(occlusion_peeking_stopline, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(occlusion_peeking_stop_line, baselink2front, *path); + planning_utils::getAheadPose(occlusion_peeking_stopline, baselink2front, *path); debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(occlusion_peeking_stop_line).point.pose; + stop_factor.stop_pose = path->points.at(occlusion_peeking_stopline).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(occlusion_peeking_stop_line).point.pose, VelocityFactor::INTERSECTION); + path->points.at(occlusion_peeking_stopline).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.collision_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.collision_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -640,35 +642,35 @@ void reactRTCApprovalByDecisionResult( "OccludedCollisionStop, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.collision_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.collision_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.temporal_stop_before_attention_required - ? decision_result.first_attention_stop_line_idx - : decision_result.occlusion_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.temporal_stop_before_attention_required + ? decision_result.first_attention_stopline_idx + : decision_result.occlusion_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); debug_data->static_occlusion_with_traffic_light_timeout = decision_result.static_occlusion_timeout; { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -687,31 +689,31 @@ void reactRTCApprovalByDecisionResult( "OccludedAbsenceTrafficLight, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.closest_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.closest_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && decision_result.temporal_stop_before_attention_required) { - const auto stop_line_idx = decision_result.first_attention_area_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.first_attention_area_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && !decision_result.temporal_stop_before_attention_required) { @@ -719,7 +721,7 @@ void reactRTCApprovalByDecisionResult( const auto peeking_limit_line = decision_result.peeking_limit_line_idx; for (auto i = closest_idx; i <= peeking_limit_line; ++i) { planning_utils::setVelocityFromIndex( - i, planner_param.occlusion.absence_traffic_light.creep_velocity, path); + i, planner_param.occlusion.creep_velocity_without_traffic_light, path); } debug_data->absence_traffic_light_creep_wall = planning_utils::getAheadPose(closest_idx, baselink2front, *path); @@ -739,31 +741,31 @@ void reactRTCApprovalByDecisionResult( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), "Safe, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.collision_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.collision_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.occlusion_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -772,41 +774,41 @@ void reactRTCApprovalByDecisionResult( template <> void reactRTCApprovalByDecisionResult( const bool rtc_default_approved, const bool rtc_occlusion_approved, - const IntersectionModule::TrafficLightArrowSolidOn & decision_result, + const IntersectionModule::FullyPrioritized & decision_result, [[maybe_unused]] const IntersectionModule::PlannerParam & planner_param, const double baselink2front, autoware_auto_planning_msgs::msg::PathWithLaneId * path, StopReason * stop_reason, VelocityFactorInterface * velocity_factor, util::DebugData * debug_data) { RCLCPP_DEBUG( rclcpp::get_logger("reactRTCApprovalByDecisionResult"), - "TrafficLightArrowSolidOn, approval = (default: %d, occlusion: %d)", rtc_default_approved, + "FullyPrioritized, approval = (default: %d, occlusion: %d)", rtc_default_approved, rtc_occlusion_approved); if (!rtc_default_approved) { - const auto stop_line_idx = decision_result.collision_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.collision_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->collision_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } if (!rtc_occlusion_approved && planner_param.occlusion.enable) { - const auto stop_line_idx = decision_result.occlusion_stop_line_idx; - planning_utils::setVelocityFromIndex(stop_line_idx, 0.0, path); + const auto stopline_idx = decision_result.occlusion_stopline_idx; + planning_utils::setVelocityFromIndex(stopline_idx, 0.0, path); debug_data->occlusion_stop_wall_pose = - planning_utils::getAheadPose(stop_line_idx, baselink2front, *path); + planning_utils::getAheadPose(stopline_idx, baselink2front, *path); { tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = path->points.at(stop_line_idx).point.pose; + stop_factor.stop_pose = path->points.at(stopline_idx).point.pose; planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor->set( path->points, path->points.at(decision_result.closest_idx).point.pose, - path->points.at(stop_line_idx).point.pose, VelocityFactor::INTERSECTION); + path->points.at(stopline_idx).point.pose, VelocityFactor::UNKNOWN); } } return; @@ -859,8 +861,8 @@ static std::string formatDecisionResult(const IntersectionModule::DecisionResult if (std::holds_alternative(decision_result)) { return "OccludedAbsenceTrafficLight"; } - if (std::holds_alternative(decision_result)) { - return "TrafficLightArrowSolidOn"; + if (std::holds_alternative(decision_result)) { + return "FullyPrioritized"; } return ""; } @@ -959,7 +961,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( lanelet_map_ptr, routing_graph_ptr, assigned_lanelet, lanelets_on_path, associative_ids_, planner_param_.common.attention_area_length, planner_param_.occlusion.occlusion_attention_area_length, - planner_param_.common.consider_wrong_direction_vehicle); + planner_param_.collision_detection.consider_wrong_direction_vehicle); } auto & intersection_lanelets = intersection_lanelets_.value(); @@ -993,20 +995,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( intersection_lanelets.first_attention_lane() ? intersection_lanelets.first_attention_lane().value().centerline2d() : first_conflicting_lane.centerline2d(); - const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( + const auto intersection_stoplines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, dummy_first_attention_lane_centerline, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, - planner_param_.common.stop_line_margin, planner_param_.common.max_accel, + planner_param_.common.default_stopline_margin, planner_param_.common.max_accel, planner_param_.common.max_jerk, planner_param_.common.delay_response_time, planner_param_.occlusion.peeking_offset, path); - if (!intersection_stop_lines_opt) { - return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; + if (!intersection_stoplines_opt) { + return IntersectionModule::Indecisive{"failed to generate intersection_stoplines"}; } - const auto & intersection_stop_lines = intersection_stop_lines_opt.value(); + const auto & intersection_stoplines = intersection_stoplines_opt.value(); const auto - [closest_idx, stuck_stop_line_idx_opt, default_stop_line_idx_opt, - first_attention_stop_line_idx_opt, occlusion_peeking_stop_line_idx_opt, - default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stop_lines; + [closest_idx, stuck_stopline_idx_opt, default_stopline_idx_opt, + first_attention_stopline_idx_opt, occlusion_peeking_stopline_idx_opt, + default_pass_judge_line_idx, occlusion_wo_tl_pass_judge_line_idx] = intersection_stoplines; // see the doc for struct PathLanelets const auto & conflicting_area = intersection_lanelets.conflicting_area(); @@ -1027,7 +1029,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( [&](const size_t pos, const double duration, StateMachine & state_machine) { const double dist_stopline = fromEgoDist(pos); const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); const bool over_stopline = (dist_stopline < 0.0); const bool is_stopped_duration = planner_data_->isVehicleStopped(duration); if (over_stopline) { @@ -1040,8 +1042,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( auto stoppedAtPosition = [&](const size_t pos, const double duration) { const double dist_stopline = fromEgoDist(pos); const bool approached_dist_stopline = - (std::fabs(dist_stopline) < planner_param_.common.stop_overshoot_margin); - const bool over_stopline = (dist_stopline < -planner_param_.common.stop_overshoot_margin); + (std::fabs(dist_stopline) < planner_param_.common.stopline_overshoot_margin); + const bool over_stopline = (dist_stopline < -planner_param_.common.stopline_overshoot_margin); const bool is_stopped = planner_data_->isVehicleStopped(duration); if (over_stopline) { return true; @@ -1054,17 +1056,17 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // stuck vehicle detection is viable even if attention area is empty // so this needs to be checked before attention area validation const bool stuck_detected = checkStuckVehicle(planner_data_, path_lanelets); - if (stuck_detected && stuck_stop_line_idx_opt) { - auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); + if (stuck_detected && stuck_stopline_idx_opt) { + auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); if (is_private_area_ && planner_param_.stuck_vehicle.enable_private_area_stuck_disregard) { if ( - default_stop_line_idx_opt && - fromEgoDist(stuck_stop_line_idx) < -planner_param_.common.stop_overshoot_margin) { - stuck_stop_line_idx = default_stop_line_idx_opt.value(); + default_stopline_idx_opt && + fromEgoDist(stuck_stopline_idx) < -planner_param_.common.stopline_overshoot_margin) { + stuck_stopline_idx = default_stopline_idx_opt.value(); } } else { return IntersectionModule::StuckStop{ - closest_idx, stuck_stop_line_idx, occlusion_peeking_stop_line_idx_opt}; + closest_idx, stuck_stopline_idx, occlusion_peeking_stopline_idx_opt}; } } @@ -1072,9 +1074,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // so this needs to be checked before attention area validation const bool yield_stuck_detected = checkYieldStuckVehicle( planner_data_, path_lanelets, intersection_lanelets.first_attention_area()); - if (yield_stuck_detected && stuck_stop_line_idx_opt) { - const auto stuck_stop_line_idx = stuck_stop_line_idx_opt.value(); - return IntersectionModule::YieldStuckStop{closest_idx, stuck_stop_line_idx}; + if (yield_stuck_detected && stuck_stopline_idx_opt) { + const auto stuck_stopline_idx = stuck_stopline_idx_opt.value(); + return IntersectionModule::YieldStuckStop{closest_idx, stuck_stopline_idx}; } // if attention area is empty, collision/occlusion detection is impossible @@ -1085,21 +1087,20 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // if attention area is not null but default stop line is not available, ego/backward-path has // already passed the stop line - if (!default_stop_line_idx_opt) { + if (!default_stopline_idx_opt) { return IntersectionModule::Indecisive{"default stop line is null"}; } // occlusion stop line is generated from the intersection of ego footprint along the path with the // attention area, so if this is null, eog has already passed the intersection - if (!first_attention_stop_line_idx_opt || !occlusion_peeking_stop_line_idx_opt) { + if (!first_attention_stopline_idx_opt || !occlusion_peeking_stopline_idx_opt) { return IntersectionModule::Indecisive{"occlusion stop line is null"}; } - const auto default_stop_line_idx = default_stop_line_idx_opt.value(); - const bool is_over_default_stop_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, default_stop_line_idx); - const auto collision_stop_line_idx = - is_over_default_stop_line ? closest_idx : default_stop_line_idx; - const auto first_attention_stop_line_idx = first_attention_stop_line_idx_opt.value(); - const auto occlusion_stop_line_idx = occlusion_peeking_stop_line_idx_opt.value(); + const auto default_stopline_idx = default_stopline_idx_opt.value(); + const bool is_over_default_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose, default_stopline_idx); + const auto collision_stopline_idx = is_over_default_stopline ? closest_idx : default_stopline_idx; + const auto first_attention_stopline_idx = first_attention_stopline_idx_opt.value(); + const auto occlusion_stopline_idx = occlusion_peeking_stopline_idx_opt.value(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); const auto & occlusion_attention_lanelets = intersection_lanelets.occlusion_attention(); @@ -1123,15 +1124,16 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // filter objects auto target_objects = generateTargetObjects(intersection_lanelets, intersection_area); - const double occlusion_dist_thr = std::fabs( - std::pow(planner_param_.occlusion.max_vehicle_velocity_for_rss, 2) / - (2 * planner_param_.occlusion.min_vehicle_brake_for_rss)); + const double is_amber_or_red = + (traffic_prioritized_level == util::TrafficPrioritizedLevel::PARTIALLY_PRIORITIZED) || + (traffic_prioritized_level == util::TrafficPrioritizedLevel::FULLY_PRIORITIZED); auto occlusion_status = - (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_prioritized) + (enable_occlusion_detection_ && !occlusion_attention_lanelets.empty() && !is_amber_or_red) ? getOcclusionStatus( *planner_data_->occupancy_grid, occlusion_attention_area, adjacent_lanelets, first_attention_area, interpolated_path_info, occlusion_attention_divisions, - target_objects, current_pose, occlusion_dist_thr) + target_objects, current_pose, + planner_param_.occlusion.occlusion_required_clearance_distance) : OcclusionType::NOT_OCCLUDED; occlusion_stop_state_machine_.setStateWithMarginTime( occlusion_status == OcclusionType::NOT_OCCLUDED ? StateMachine::State::GO : StateMachine::STOP, @@ -1156,7 +1158,7 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( // if occlusion detection is enabled, pass_judge position is beyond the boundary of first // attention area if (has_traffic_light_) { - return occlusion_stop_line_idx; + return occlusion_stopline_idx; } else if (is_occlusion_state) { // if there is no traffic light and occlusion is detected, pass_judge position is beyond // the boundary of first attention area @@ -1177,15 +1179,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( planner_data_->current_velocity->twist.linear.x, planner_data_->current_velocity->twist.linear.y); const bool keep_detection = - (vel_norm < planner_param_.collision_detection.keep_detection_vel_thr); + (vel_norm < planner_param_.collision_detection.keep_detection_velocity_threshold); const bool was_safe = std::holds_alternative(prev_decision_result_); // if ego is over the pass judge line and not stopped - if (is_over_default_stop_line && !is_over_pass_judge_line && keep_detection) { - RCLCPP_DEBUG( - logger_, "is_over_default_stop_line && !is_over_pass_judge_line && keep_detection"); + if (is_over_default_stopline && !is_over_pass_judge_line && keep_detection) { + RCLCPP_DEBUG(logger_, "is_over_default_stopline && !is_over_pass_judge_line && keep_detection"); // do nothing } else if ( - (was_safe && is_over_default_stop_line && is_over_pass_judge_line && is_go_out_) || + (was_safe && is_over_default_stopline && is_over_pass_judge_line && is_go_out_) || is_permanent_go_) { // is_go_out_: previous RTC approval // activated_: current RTC approval @@ -1218,8 +1219,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool exist_close_vehicles = std::any_of( target_objects.all_attention_objects.begin(), target_objects.all_attention_objects.end(), [&](const auto & object) { - return object.dist_to_stop_line.has_value() && - object.dist_to_stop_line.value() < + return object.dist_to_stopline.has_value() && + object.dist_to_stopline.value() < planner_param_.collision_detection.yield_on_green_traffic_light .object_dist_to_stopline; }); @@ -1228,20 +1229,21 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( rclcpp::Duration((now - initial_green_light_observed_time_.value())).seconds() < planner_param_.collision_detection.yield_on_green_traffic_light.duration) { return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; } } } // calculate dynamic collision around attention area - const double time_to_restart = (is_go_out_ || is_prioritized) - ? 0.0 - : (planner_param_.collision_detection.state_transit_margin_time - - collision_state_machine_.getDuration()); + const double time_to_restart = + (is_go_out_ || is_prioritized) + ? 0.0 + : (planner_param_.collision_detection.collision_detection_hold_time - + collision_state_machine_.getDuration()); const bool has_collision = checkCollision( *path, &target_objects, path_lanelets, closest_idx, - std::min(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart, + std::min(occlusion_stopline_idx, path->points.size() - 1), time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, @@ -1250,23 +1252,23 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( collision_state_machine_.getState() == StateMachine::State::STOP; if (is_prioritized) { - return TrafficLightArrowSolidOn{ - has_collision_with_margin, closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + return FullyPrioritized{ + has_collision_with_margin, closest_idx, collision_stopline_idx, occlusion_stopline_idx}; } // Safe if (!is_occlusion_state && !has_collision_with_margin) { - return IntersectionModule::Safe{closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; } // Only collision if (!is_occlusion_state && has_collision_with_margin) { return IntersectionModule::NonOccludedCollisionStop{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + closest_idx, collision_stopline_idx, occlusion_stopline_idx}; } // Occluded // occlusion_status is assured to be not NOT_OCCLUDED const bool stopped_at_default_line = stoppedForDuration( - default_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + default_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking, before_creep_state_machine_); if (stopped_at_default_line) { // if specified the parameter occlusion.temporal_stop_before_attention_area OR @@ -1274,7 +1276,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool temporal_stop_before_attention_required = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) ? !stoppedForDuration( - first_attention_stop_line_idx, planner_param_.occlusion.before_creep_stop_time, + first_attention_stopline_idx, + planner_param_.occlusion.temporal_stop_time_before_peeking, temporal_stop_before_attention_state_machine_) : false; if (!has_traffic_light_) { @@ -1283,15 +1286,19 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( "already passed maximum peeking line in the absence of traffic light"}; } return IntersectionModule::OccludedAbsenceTrafficLight{ - is_occlusion_cleared_with_margin, has_collision_with_margin, - temporal_stop_before_attention_required, closest_idx, - first_attention_stop_line_idx, occlusion_wo_tl_pass_judge_line_idx}; + is_occlusion_cleared_with_margin, + has_collision_with_margin, + temporal_stop_before_attention_required, + closest_idx, + first_attention_stopline_idx, + occlusion_wo_tl_pass_judge_line_idx}; } // following remaining block is "has_traffic_light_" // if ego is stuck by static occlusion in the presence of traffic light, start timeout count const bool is_static_occlusion = occlusion_status == OcclusionType::STATICALLY_OCCLUDED; const bool is_stuck_by_static_occlusion = - stoppedAtPosition(occlusion_stop_line_idx, planner_param_.occlusion.before_creep_stop_time) && + stoppedAtPosition( + occlusion_stopline_idx, planner_param_.occlusion.temporal_stop_time_before_peeking) && is_static_occlusion; if (has_collision_with_margin) { // if collision is detected, timeout is reset @@ -1303,13 +1310,12 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const bool release_static_occlusion_stuck = (static_occlusion_timeout_state_machine_.getState() == StateMachine::State::GO); if (!has_collision_with_margin && release_static_occlusion_stuck) { - return IntersectionModule::Safe{ - closest_idx, collision_stop_line_idx, occlusion_stop_line_idx}; + return IntersectionModule::Safe{closest_idx, collision_stopline_idx, occlusion_stopline_idx}; } // occlusion_status is either STATICALLY_OCCLUDED or DYNAMICALLY_OCCLUDED const double max_timeout = planner_param_.occlusion.static_occlusion_with_traffic_light_timeout + - planner_param_.occlusion.stop_release_margin_time; + planner_param_.occlusion.occlusion_detection_hold_time; const std::optional static_occlusion_timeout = is_stuck_by_static_occlusion ? std::make_optional( @@ -1317,29 +1323,31 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( occlusion_stop_state_machine_.getDuration()) : (is_static_occlusion ? std::make_optional(max_timeout) : std::nullopt); if (has_collision_with_margin) { - return IntersectionModule::OccludedCollisionStop{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx, - static_occlusion_timeout}; + return IntersectionModule::OccludedCollisionStop{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; } else { - return IntersectionModule::PeekingTowardOcclusion{is_occlusion_cleared_with_margin, - temporal_stop_before_attention_required, - closest_idx, - collision_stop_line_idx, - first_attention_stop_line_idx, - occlusion_stop_line_idx, - static_occlusion_timeout}; + return IntersectionModule::PeekingTowardOcclusion{ + is_occlusion_cleared_with_margin, + temporal_stop_before_attention_required, + closest_idx, + collision_stopline_idx, + first_attention_stopline_idx, + occlusion_stopline_idx, + static_occlusion_timeout}; } } else { - const auto occlusion_stop_line = + const auto occlusion_stopline = (planner_param_.occlusion.temporal_stop_before_attention_area || !has_traffic_light_) - ? first_attention_stop_line_idx - : occlusion_stop_line_idx; + ? first_attention_stopline_idx + : occlusion_stopline_idx; return IntersectionModule::FirstWaitBeforeOcclusion{ - is_occlusion_cleared_with_margin, closest_idx, default_stop_line_idx, occlusion_stop_line}; + is_occlusion_cleared_with_margin, closest_idx, default_stopline_idx, occlusion_stopline}; } } @@ -1363,8 +1371,8 @@ bool IntersectionModule::checkStuckVehicle( debug_data_.stuck_vehicle_detect_area = toGeomPoly(stuck_vehicle_detect_area); return util::checkStuckVehicleInIntersection( - objects_ptr, stuck_vehicle_detect_area, planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - &debug_data_); + objects_ptr, stuck_vehicle_detect_area, + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, &debug_data_); } bool IntersectionModule::checkYieldStuckVehicle( @@ -1376,12 +1384,9 @@ bool IntersectionModule::checkYieldStuckVehicle( } const bool yield_stuck_detection_direction = [&]() { - return (turn_direction_ == "left" && - planner_param_.stuck_vehicle.yield_stuck_turn_direction.left) || - (turn_direction_ == "right" && - planner_param_.stuck_vehicle.yield_stuck_turn_direction.right) || - (turn_direction_ == "straight" && - planner_param_.stuck_vehicle.yield_stuck_turn_direction.straight); + return (turn_direction_ == "left" && planner_param_.yield_stuck.turn_direction.left) || + (turn_direction_ == "right" && planner_param_.yield_stuck.turn_direction.right) || + (turn_direction_ == "straight" && planner_param_.yield_stuck.turn_direction.straight); }(); if (!yield_stuck_detection_direction) { return false; @@ -1394,8 +1399,8 @@ bool IntersectionModule::checkYieldStuckVehicle( return util::checkYieldStuckVehicleInIntersection( objects_ptr, ego_poly, first_attention_area.value(), - planner_param_.stuck_vehicle.stuck_vehicle_vel_thr, - planner_param_.stuck_vehicle.yield_stuck_distance_thr, &debug_data_); + planner_param_.stuck_vehicle.stuck_vehicle_velocity_threshold, + planner_param_.yield_stuck.distance_threshold, &debug_data_); } util::TargetObjects IntersectionModule::generateTargetObjects( @@ -1407,7 +1412,7 @@ util::TargetObjects IntersectionModule::generateTargetObjects( util::TargetObjects target_objects; target_objects.header = objects_ptr->header; const auto & attention_lanelets = intersection_lanelets.attention(); - const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stop_lines(); + const auto & attention_lanelet_stoplines = intersection_lanelets.attention_stoplines(); const auto & adjacent_lanelets = intersection_lanelets.adjacent(); for (const auto & object : objects_ptr->objects) { // ignore non-vehicle type objects, such as pedestrian. @@ -1418,8 +1423,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects( // check direction of objects const auto object_direction = util::getObjectPoseWithVelocityDirection(object.kinematics); const auto belong_adjacent_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, + object_direction, adjacent_lanelets, planner_param_.common.attention_area_angle_threshold, + planner_param_.collision_detection.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin, false); if (belong_adjacent_lanelet_id) { continue; @@ -1435,27 +1440,27 @@ util::TargetObjects IntersectionModule::generateTargetObjects( const auto obj_poly = tier4_autoware_utils::toPolygon2d(object); const auto intersection_area_2d = intersection_area.value(); const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( - object_direction, attention_lanelets, planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, + object_direction, attention_lanelets, planner_param_.common.attention_area_angle_threshold, + planner_param_.collision_detection.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin, is_parked_vehicle); if (belong_attention_lanelet_id) { const auto id = belong_attention_lanelet_id.value(); util::TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stop_line = attention_lanelet_stoplines.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } else if (bg::within(Point2d{obj_pos.x, obj_pos.y}, intersection_area_2d)) { util::TargetObject target_object; target_object.object = object; target_object.attention_lanelet = std::nullopt; - target_object.stop_line = std::nullopt; + target_object.stopline = std::nullopt; target_objects.intersection_area_objects.push_back(target_object); } } else if (const auto belong_attention_lanelet_id = util::checkAngleForTargetLanelets( object_direction, attention_lanelets, - planner_param_.common.attention_area_angle_thr, - planner_param_.common.consider_wrong_direction_vehicle, + planner_param_.common.attention_area_angle_threshold, + planner_param_.collision_detection.consider_wrong_direction_vehicle, planner_param_.common.attention_area_margin, is_parked_vehicle); belong_attention_lanelet_id.has_value()) { // intersection_area is not available, use detection_area_with_margin as before @@ -1463,7 +1468,7 @@ util::TargetObjects IntersectionModule::generateTargetObjects( util::TargetObject target_object; target_object.object = object; target_object.attention_lanelet = attention_lanelets.at(id); - target_object.stop_line = attention_lanelet_stoplines.at(id); + target_object.stopline = attention_lanelet_stoplines.at(id); container.push_back(target_object); } } @@ -1474,7 +1479,7 @@ util::TargetObjects IntersectionModule::generateTargetObjects( target_objects.all_attention_objects.push_back(object); } for (auto & object : target_objects.all_attention_objects) { - object.calc_dist_to_stop_line(); + object.calc_dist_to_stopline(); } return target_objects; } @@ -1482,7 +1487,7 @@ util::TargetObjects IntersectionModule::generateTargetObjects( bool IntersectionModule::checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level) { using lanelet::utils::getArcCoordinates; @@ -1493,11 +1498,12 @@ bool IntersectionModule::checkCollision( tier4_debug_msgs::msg::Float64MultiArrayStamped ego_ttc_time_array; const auto time_distance_array = util::calcIntersectionPassingTime( path, planner_data_, lane_id_, associative_ids_, closest_idx, - last_intersection_stop_line_candidate_idx, time_delay, - planner_param_.common.intersection_velocity, - planner_param_.collision_detection.minimum_ego_predicted_velocity, - planner_param_.collision_detection.use_upstream_velocity, - planner_param_.collision_detection.minimum_upstream_velocity, &ego_ttc_time_array); + last_intersection_stopline_candidate_idx, time_delay, + planner_param_.collision_detection.velocity_profile.default_velocity, + planner_param_.collision_detection.velocity_profile.minimum_default_velocity, + planner_param_.collision_detection.velocity_profile.use_upstream, + planner_param_.collision_detection.velocity_profile.minimum_upstream_velocity, + &ego_ttc_time_array); if ( std::find(planner_param_.debug.ttc.begin(), planner_param_.debug.ttc.end(), lane_id_) != @@ -1533,11 +1539,11 @@ bool IntersectionModule::checkCollision( planner_param_.collision_detection.not_prioritized.collision_end_margin_time); }(); const auto expectedToStopBeforeStopLine = [&](const util::TargetObject & target_object) { - if (!target_object.dist_to_stop_line) { + if (!target_object.dist_to_stopline) { return false; } - const double dist_to_stop_line = target_object.dist_to_stop_line.value(); - if (dist_to_stop_line < 0) { + const double dist_to_stopline = target_object.dist_to_stopline.value(); + if (dist_to_stopline < 0) { return false; } const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; @@ -1545,25 +1551,25 @@ bool IntersectionModule::checkCollision( v * v / (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light .object_expected_deceleration)); - return dist_to_stop_line > braking_distance; + return dist_to_stopline > braking_distance; }; const auto isTolerableOvershoot = [&](const util::TargetObject & target_object) { if ( - !target_object.attention_lanelet || !target_object.dist_to_stop_line || - !target_object.stop_line) { + !target_object.attention_lanelet || !target_object.dist_to_stopline || + !target_object.stopline) { return false; } - const double dist_to_stop_line = target_object.dist_to_stop_line.value(); + const double dist_to_stopline = target_object.dist_to_stopline.value(); const double v = target_object.object.kinematics.initial_twist_with_covariance.twist.linear.x; const double braking_distance = v * v / (2.0 * std::fabs(planner_param_.collision_detection.ignore_on_amber_traffic_light .object_expected_deceleration)); - if (dist_to_stop_line > braking_distance) { + if (dist_to_stopline > braking_distance) { return false; } - const auto stopline_front = target_object.stop_line.value().front(); - const auto stopline_back = target_object.stop_line.value().back(); + const auto stopline_front = target_object.stopline.value().front(); + const auto stopline_back = target_object.stopline.value().back(); tier4_autoware_utils::LineString2d object_line; object_line.emplace_back( (stopline_front.x() + stopline_back.x()) / 2.0, @@ -1578,7 +1584,7 @@ bool IntersectionModule::checkCollision( } const auto collision_point = intersections.front(); // distance from object expected stop position to collision point - const double stopline_to_object = -1.0 * dist_to_stop_line + braking_distance; + const double stopline_to_object = -1.0 * dist_to_stopline + braking_distance; const double stopline_to_collision = std::hypot(collision_point.x() - stopline_mid.x(), collision_point.y() - stopline_mid.y()); const double object2collision = stopline_to_collision - stopline_to_object; diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c794ef6c53aad..4c33c0960afc3 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -49,66 +49,69 @@ class IntersectionModule : public SceneModuleInterface { struct Common { - double attention_area_margin; //! used for detecting objects in attention area - double attention_area_length; //! used to create attention area polygon - double attention_area_angle_thr; //! threshold in checking the angle of detecting objects - double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary - double intersection_velocity; //! used for intersection passing time - double intersection_max_acc; //! used for calculating intersection velocity - double stop_overshoot_margin; //! overshoot margin for stuck, collision detection + double attention_area_length; + double attention_area_margin; + double attention_area_angle_threshold; bool use_intersection_area; - bool consider_wrong_direction_vehicle; + double default_stopline_margin; + double stopline_overshoot_margin; double path_interpolation_ds; double max_accel; double max_jerk; double delay_response_time; } common; + + struct TurnDirection + { + bool left; + bool right; + bool straight; + }; + struct StuckVehicle { - struct TurnDirection - { - bool left; - bool right; - bool straight; - }; TurnDirection turn_direction; - bool use_stuck_stopline; //! stopline generate before the intersection lanelet when is_stuck. - double stuck_vehicle_detect_dist; //! distance from end point to finish stuck vehicle check - double stuck_vehicle_vel_thr; //! Threshold of the speed to be recognized as stopped - /* - double - assumed_front_car_decel; //! the expected deceleration of front car when front car as well - bool enable_front_car_decel_prediction; //! flag for using above feature - */ + bool use_stuck_stopline; + double stuck_vehicle_detect_dist; + double stuck_vehicle_velocity_threshold; double timeout_private_area; bool enable_private_area_stuck_disregard; - double yield_stuck_distance_thr; - TurnDirection yield_stuck_turn_direction; } stuck_vehicle; + + struct YieldStuck + { + TurnDirection turn_direction; + double distance_threshold; + } yield_stuck; + struct CollisionDetection { + bool consider_wrong_direction_vehicle; + double collision_detection_hold_time; double min_predicted_path_confidence; - //! minimum confidence value of predicted path to use for collision detection - double minimum_ego_predicted_velocity; //! used to calculate ego's future velocity profile - double state_transit_margin_time; + double keep_detection_velocity_threshold; + struct VelocityProfile + { + bool use_upstream; + double minimum_upstream_velocity; + double default_velocity; + double minimum_default_velocity; + } velocity_profile; struct FullyPrioritized { - double collision_start_margin_time; //! start margin time to check collision - double collision_end_margin_time; //! end margin time to check collision + double collision_start_margin_time; + double collision_end_margin_time; } fully_prioritized; struct PartiallyPrioritized { - double collision_start_margin_time; //! start margin time to check collision - double collision_end_margin_time; //! end margin time to check collision + double collision_start_margin_time; + double collision_end_margin_time; } partially_prioritized; struct NotPrioritized { - double collision_start_margin_time; //! start margin time to check collision - double collision_end_margin_time; //! end margin time to check collision + double collision_start_margin_time; + double collision_end_margin_time; } not_prioritized; - double keep_detection_vel_thr; //! keep detection if ego is ego.vel < keep_detection_vel_thr - bool use_upstream_velocity; - double minimum_upstream_velocity; struct YieldOnGreeTrafficLight { double distance_to_assigned_lanelet_start; @@ -124,33 +127,32 @@ class IntersectionModule : public SceneModuleInterface double object_margin_to_path; } ignore_on_red_traffic_light; } collision_detection; + struct Occlusion { bool enable; - double occlusion_attention_area_length; //! used for occlusion detection - bool enable_creeping; - double occlusion_creep_velocity; //! the creep velocity to occlusion limit stop line - double peeking_offset; + double occlusion_attention_area_length; int free_space_max; int occupied_min; - bool do_dp; - double before_creep_stop_time; - double min_vehicle_brake_for_rss; - double max_vehicle_velocity_for_rss; double denoise_kernel; + double attention_lane_crop_curvature_threshold; + double attention_lane_curvature_calculation_ds; + struct CreepDuringPeeking + { + bool enable; + double creep_velocity; + } creep_during_peeking; + double peeking_offset; + double occlusion_required_clearance_distance; std::vector possible_object_bbox; double ignore_parked_vehicle_speed_threshold; - double stop_release_margin_time; + double occlusion_detection_hold_time; + double temporal_stop_time_before_peeking; bool temporal_stop_before_attention_area; - struct AbsenceTrafficLight - { - double creep_velocity; - double maximum_peeking_distance; - } absence_traffic_light; - double attention_lane_crop_curvature_threshold; - double attention_lane_curvature_calculation_ds; + double creep_velocity_without_traffic_light; double static_occlusion_with_traffic_light_timeout; } occlusion; + struct Debug { std::vector ttc; @@ -171,26 +173,26 @@ class IntersectionModule : public SceneModuleInterface struct StuckStop { size_t closest_idx{0}; - size_t stuck_stop_line_idx{0}; - std::optional occlusion_stop_line_idx{std::nullopt}; + size_t stuck_stopline_idx{0}; + std::optional occlusion_stopline_idx{std::nullopt}; }; struct YieldStuckStop { size_t closest_idx{0}; - size_t stuck_stop_line_idx{0}; + size_t stuck_stopline_idx{0}; }; struct NonOccludedCollisionStop { size_t closest_idx{0}; - size_t collision_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; }; struct FirstWaitBeforeOcclusion { bool is_actually_occlusion_cleared{false}; size_t closest_idx{0}; - size_t first_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t first_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; }; // A state peeking to occlusion limit line in the presence of traffic light struct PeekingTowardOcclusion @@ -200,9 +202,9 @@ class IntersectionModule : public SceneModuleInterface bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; - size_t collision_stop_line_idx{0}; - size_t first_attention_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) // if valid, it contains the remaining time to release the static occlusion stuck and shows up // intersection_occlusion(x.y) @@ -214,9 +216,9 @@ class IntersectionModule : public SceneModuleInterface bool is_actually_occlusion_cleared{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; - size_t collision_stop_line_idx{0}; - size_t first_attention_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t collision_stopline_idx{0}; + size_t first_attention_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; // if null, it is dynamic occlusion and shows up intersection_occlusion(dyn) // if valid, it contains the remaining time to release the static occlusion stuck std::optional static_occlusion_timeout{std::nullopt}; @@ -227,22 +229,22 @@ class IntersectionModule : public SceneModuleInterface bool collision_detected{false}; bool temporal_stop_before_attention_required{false}; size_t closest_idx{0}; - size_t first_attention_area_stop_line_idx{0}; + size_t first_attention_area_stopline_idx{0}; size_t peeking_limit_line_idx{0}; }; struct Safe { // NOTE: if RTC is disapproved status, default stop lines are still needed. size_t closest_idx{0}; - size_t collision_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; }; - struct TrafficLightArrowSolidOn + struct FullyPrioritized { bool collision_detected{false}; size_t closest_idx{0}; - size_t collision_stop_line_idx{0}; - size_t occlusion_stop_line_idx{0}; + size_t collision_stopline_idx{0}; + size_t occlusion_stopline_idx{0}; }; using DecisionResult = std::variant< Indecisive, // internal process error, or over the pass judge line @@ -254,12 +256,12 @@ class IntersectionModule : public SceneModuleInterface OccludedCollisionStop, // occlusion and collision are both detected OccludedAbsenceTrafficLight, // occlusion is detected in the absence of traffic light Safe, // judge as safe - TrafficLightArrowSolidOn // only detect vehicles violating traffic rules + FullyPrioritized // only detect vehicles violating traffic rules >; IntersectionModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & associative_ids, + const PlannerParam & planner_param, const std::set & associative_ids, const std::string & turn_direction, const bool has_traffic_light, const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -273,7 +275,7 @@ class IntersectionModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; - const std::set & getAssociativeIds() const { return associative_ids_; } + const std::set & getAssociativeIds() const { return associative_ids_; } UUID getOcclusionUUID() const { return occlusion_uuid_; } bool getOcclusionSafety() const { return occlusion_safety_; } @@ -284,7 +286,7 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; const int64_t lane_id_; - const std::set associative_ids_; + const std::set associative_ids_; const std::string turn_direction_; const bool has_traffic_light_; @@ -345,7 +347,7 @@ class IntersectionModule : public SceneModuleInterface bool checkCollision( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets, - const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx, + const size_t closest_idx, const size_t last_intersection_stopline_candidate_idx, const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level); OcclusionType getOcclusionStatus( diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp index f1b516d72726f..b373f2cbc1c8a 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp @@ -39,13 +39,13 @@ namespace bg = boost::geometry; MergeFromPrivateRoadModule::MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, [[maybe_unused]] std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & associative_ids, + const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), lane_id_(lane_id), associative_ids_(associative_ids) { - velocity_factor_.init(VelocityFactor::MERGE); + velocity_factor_.init(PlanningBehavior::MERGE); planner_param_ = planner_param; state_machine_.setState(StateMachine::State::STOP); } @@ -105,39 +105,39 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(PathWithLaneId * path, StopR } /* set stop-line and stop-judgement-line for base_link */ - const auto stop_line_idx_opt = util::generateStuckStopLine( + const auto stopline_idx_opt = util::generateStuckStopLine( first_conflicting_area.value(), planner_data_, interpolated_path_info, - planner_param_.stop_line_margin, false, path); - if (!stop_line_idx_opt.has_value()) { + planner_param_.stopline_margin, false, path); + if (!stopline_idx_opt.has_value()) { RCLCPP_WARN_SKIPFIRST_THROTTLE(logger_, *clock_, 1000 /* ms */, "setStopLineIdx fail"); return false; } - const size_t stop_line_idx = stop_line_idx_opt.value(); - if (stop_line_idx == 0) { + const size_t stopline_idx = stopline_idx_opt.value(); + if (stopline_idx == 0) { RCLCPP_DEBUG(logger_, "stop line is at path[0], ignore planning."); return true; } debug_data_.virtual_wall_pose = planning_utils::getAheadPose( - stop_line_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); - debug_data_.stop_point_pose = path->points.at(stop_line_idx).point.pose; + stopline_idx, planner_data_->vehicle_info_.max_longitudinal_offset_m, *path); + debug_data_.stop_point_pose = path->points.at(stopline_idx).point.pose; /* set stop speed */ if (state_machine_.getState() == StateMachine::State::STOP) { constexpr double v = 0.0; - planning_utils::setVelocityFromIndex(stop_line_idx, v, path); + planning_utils::setVelocityFromIndex(stopline_idx, v, path); /* get stop point and stop factor */ tier4_planning_msgs::msg::StopFactor stop_factor; stop_factor.stop_pose = debug_data_.stop_point_pose; planning_utils::appendStopReason(stop_factor, stop_reason); - const auto & stop_pose = path->points.at(stop_line_idx).point.pose; + const auto & stop_pose = path->points.at(stopline_idx).point.pose; velocity_factor_.set( path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::UNKNOWN); const double signed_arc_dist_to_stop_point = motion_utils::calcSignedArcLength( - path->points, current_pose.position, path->points.at(stop_line_idx).point.pose.position); + path->points, current_pose.position, path->points.at(stopline_idx).point.pose.position); if ( signed_arc_dist_to_stop_point < planner_param_.stop_distance_threshold && diff --git a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp index 19e7b9201093f..fab0303640700 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp @@ -56,7 +56,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface struct PlannerParam { double attention_area_length; - double stop_line_margin; + double stopline_margin; double stop_duration_sec; double stop_distance_threshold; double path_interpolation_ds; @@ -66,7 +66,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface MergeFromPrivateRoadModule( const int64_t module_id, const int64_t lane_id, std::shared_ptr planner_data, - const PlannerParam & planner_param, const std::set & associative_ids, + const PlannerParam & planner_param, const std::set & associative_ids, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); /** @@ -78,11 +78,11 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; motion_utils::VirtualWalls createVirtualWalls() override; - const std::set & getAssociativeIds() const { return associative_ids_; } + const std::set & getAssociativeIds() const { return associative_ids_; } private: const int64_t lane_id_; - const std::set associative_ids_; + const std::set associative_ids_; autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length); diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index 093f90e4b3df7..1c7e366347fec 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -117,7 +117,8 @@ static std::optional insertPointIndex( } bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids) + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, + const std::set & ids) { for (const auto & pid : p.lane_ids) { if (ids.find(pid) != ids.end()) { @@ -128,7 +129,7 @@ bool hasLaneIds( } std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids) { bool found = false; size_t start = 0; @@ -170,22 +171,22 @@ static std::optional getStopLineIndexFromMap( planner_data->route_handler_->getLaneletMapPtr()->laneletLayer.get( interpolated_path_info.lane_id); const auto road_markings = lanelet.regulatoryElementsAs(); - lanelet::ConstLineStrings3d stop_line; + lanelet::ConstLineStrings3d stopline; for (const auto & road_marking : road_markings) { const std::string type = road_marking->roadMarking().attributeOr(lanelet::AttributeName::Type, "none"); if (type == lanelet::AttributeValueString::StopLine) { - stop_line.push_back(road_marking->roadMarking()); - break; // only one stop_line exists. + stopline.push_back(road_marking->roadMarking()); + break; // only one stopline exists. } } - if (stop_line.empty()) { + if (stopline.empty()) { return std::nullopt; } - const auto p_start = stop_line.front().front(); - const auto p_end = stop_line.front().back(); - const LineString2d extended_stop_line = + const auto p_start = stopline.front().front(); + const auto p_end = stopline.front().back(); + const LineString2d extended_stopline = planning_utils::extendLine(p_start, p_end, planner_data->stop_line_extend_length); for (size_t i = lane_interval.first; i < lane_interval.second; i++) { @@ -194,7 +195,7 @@ static std::optional getStopLineIndexFromMap( const LineString2d path_segment = {{p_front.x, p_front.y}, {p_back.x, p_back.y}}; std::vector collision_points; - bg::intersection(extended_stop_line, path_segment, collision_points); + bg::intersection(extended_stopline, path_segment, collision_points); if (collision_points.empty()) { continue; @@ -268,7 +269,7 @@ std::optional generateIntersectionStopLines( const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double max_accel, const double max_jerk, + const double stopline_margin, const double max_accel, const double max_jerk, const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { @@ -277,7 +278,7 @@ std::optional generateIntersectionStopLines( const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); const double baselink2front = planner_data->vehicle_info_.max_longitudinal_offset_m; - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / ds); + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); @@ -309,19 +310,19 @@ std::optional generateIntersectionStopLines( first_footprint_attention_centerline_ip_opt.value(); // (1) default stop line position on interpolated path - bool default_stop_line_valid = true; + bool default_stopline_valid = true; int stop_idx_ip_int = -1; if (const auto map_stop_idx_ip = getStopLineIndexFromMap(interpolated_path_info, planner_data); map_stop_idx_ip) { stop_idx_ip_int = static_cast(map_stop_idx_ip.value()) - base2front_idx_dist; } if (stop_idx_ip_int < 0) { - stop_idx_ip_int = first_footprint_inside_detection_ip - stop_line_margin_idx_dist; + stop_idx_ip_int = first_footprint_inside_detection_ip - stopline_margin_idx_dist; } if (stop_idx_ip_int < 0) { - default_stop_line_valid = false; + default_stopline_valid = false; } - const auto default_stop_line_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; + const auto default_stopline_ip = stop_idx_ip_int >= 0 ? static_cast(stop_idx_ip_int) : 0; // (2) ego front stop line position on interpolated path const geometry_msgs::msg::Pose & current_pose = planner_data->current_odometry->pose; @@ -330,11 +331,11 @@ std::optional generateIntersectionStopLines( planner_data->ego_nearest_yaw_threshold); // (3) occlusion peeking stop line position on interpolated path - int occlusion_peeking_line_ip_int = static_cast(default_stop_line_ip); + int occlusion_peeking_line_ip_int = static_cast(default_stopline_ip); bool occlusion_peeking_line_valid = true; // NOTE: if footprints[0] is already inside the detection area, invalid { - const auto & base_pose0 = path_ip.points.at(default_stop_line_ip).point.pose; + const auto & base_pose0 = path_ip.points.at(default_stopline_ip).point.pose; const auto path_footprint0 = tier4_autoware_utils::transformVector( local_footprint, tier4_autoware_utils::pose2transform(base_pose0)); if (bg::intersects( @@ -349,8 +350,8 @@ std::optional generateIntersectionStopLines( const auto occlusion_peeking_line_ip = static_cast( std::clamp(occlusion_peeking_line_ip_int, 0, static_cast(path_ip.points.size()) - 1)); - const auto first_attention_stop_line_ip = first_footprint_inside_detection_ip; - const bool first_attention_stop_line_valid = true; + const auto first_attention_stopline_ip = first_footprint_inside_detection_ip; + const bool first_attention_stopline_valid = true; // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; @@ -366,52 +367,52 @@ std::optional generateIntersectionStopLines( static_cast(first_footprint_attention_centerline_ip); // (5) stuck vehicle stop line - int stuck_stop_line_ip_int = 0; - bool stuck_stop_line_valid = true; + int stuck_stopline_ip_int = 0; + bool stuck_stopline_valid = true; if (use_stuck_stopline) { // NOTE: when ego vehicle is approaching detection area and already passed // first_conflicting_area, this could be null. - const auto stuck_stop_line_idx_ip_opt = getFirstPointInsidePolygonByFootprint( + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygonByFootprint( first_conflicting_area, interpolated_path_info, local_footprint, baselink2front); - if (!stuck_stop_line_idx_ip_opt) { - stuck_stop_line_valid = false; - stuck_stop_line_ip_int = 0; + if (!stuck_stopline_idx_ip_opt) { + stuck_stopline_valid = false; + stuck_stopline_ip_int = 0; } else { - stuck_stop_line_ip_int = stuck_stop_line_idx_ip_opt.value() - stop_line_margin_idx_dist; + stuck_stopline_ip_int = stuck_stopline_idx_ip_opt.value() - stopline_margin_idx_dist; } } else { - stuck_stop_line_ip_int = - std::get<0>(lane_interval_ip) - (stop_line_margin_idx_dist + base2front_idx_dist); + stuck_stopline_ip_int = + std::get<0>(lane_interval_ip) - (stopline_margin_idx_dist + base2front_idx_dist); } - if (stuck_stop_line_ip_int < 0) { - stuck_stop_line_valid = false; + if (stuck_stopline_ip_int < 0) { + stuck_stopline_valid = false; } - const auto stuck_stop_line_ip = static_cast(std::max(0, stuck_stop_line_ip_int)); + const auto stuck_stopline_ip = static_cast(std::max(0, stuck_stopline_ip_int)); struct IntersectionStopLinesTemp { size_t closest_idx{0}; - size_t stuck_stop_line{0}; - size_t default_stop_line{0}; - size_t first_attention_stop_line{0}; - size_t occlusion_peeking_stop_line{0}; + size_t stuck_stopline{0}; + size_t default_stopline{0}; + size_t first_attention_stopline{0}; + size_t occlusion_peeking_stopline{0}; size_t pass_judge_line{0}; size_t occlusion_wo_tl_pass_judge_line{0}; }; - IntersectionStopLinesTemp intersection_stop_lines_temp; - std::list> stop_lines = { - {&closest_idx_ip, &intersection_stop_lines_temp.closest_idx}, - {&stuck_stop_line_ip, &intersection_stop_lines_temp.stuck_stop_line}, - {&default_stop_line_ip, &intersection_stop_lines_temp.default_stop_line}, - {&first_attention_stop_line_ip, &intersection_stop_lines_temp.first_attention_stop_line}, - {&occlusion_peeking_line_ip, &intersection_stop_lines_temp.occlusion_peeking_stop_line}, - {&pass_judge_line_ip, &intersection_stop_lines_temp.pass_judge_line}, + IntersectionStopLinesTemp intersection_stoplines_temp; + std::list> stoplines = { + {&closest_idx_ip, &intersection_stoplines_temp.closest_idx}, + {&stuck_stopline_ip, &intersection_stoplines_temp.stuck_stopline}, + {&default_stopline_ip, &intersection_stoplines_temp.default_stopline}, + {&first_attention_stopline_ip, &intersection_stoplines_temp.first_attention_stopline}, + {&occlusion_peeking_line_ip, &intersection_stoplines_temp.occlusion_peeking_stopline}, + {&pass_judge_line_ip, &intersection_stoplines_temp.pass_judge_line}, {&occlusion_wo_tl_pass_judge_line_ip, - &intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line}}; - stop_lines.sort( + &intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line}}; + stoplines.sort( [](const auto & it1, const auto & it2) { return *(std::get<0>(it1)) < *(std::get<0>(it2)); }); - for (const auto & [stop_idx_ip, stop_idx] : stop_lines) { + for (const auto & [stop_idx_ip, stop_idx] : stoplines) { const auto & insert_point = path_ip.points.at(*stop_idx_ip).point.pose; const auto insert_idx = insertPointIndex( insert_point, original_path, planner_data->ego_nearest_dist_threshold, @@ -422,32 +423,32 @@ std::optional generateIntersectionStopLines( *stop_idx = insert_idx.value(); } if ( - intersection_stop_lines_temp.occlusion_peeking_stop_line < - intersection_stop_lines_temp.default_stop_line) { - intersection_stop_lines_temp.occlusion_peeking_stop_line = - intersection_stop_lines_temp.default_stop_line; + intersection_stoplines_temp.occlusion_peeking_stopline < + intersection_stoplines_temp.default_stopline) { + intersection_stoplines_temp.occlusion_peeking_stopline = + intersection_stoplines_temp.default_stopline; } - IntersectionStopLines intersection_stop_lines; - intersection_stop_lines.closest_idx = intersection_stop_lines_temp.closest_idx; - if (stuck_stop_line_valid) { - intersection_stop_lines.stuck_stop_line = intersection_stop_lines_temp.stuck_stop_line; + IntersectionStopLines intersection_stoplines; + intersection_stoplines.closest_idx = intersection_stoplines_temp.closest_idx; + if (stuck_stopline_valid) { + intersection_stoplines.stuck_stopline = intersection_stoplines_temp.stuck_stopline; } - if (default_stop_line_valid) { - intersection_stop_lines.default_stop_line = intersection_stop_lines_temp.default_stop_line; + if (default_stopline_valid) { + intersection_stoplines.default_stopline = intersection_stoplines_temp.default_stopline; } - if (first_attention_stop_line_valid) { - intersection_stop_lines.first_attention_stop_line = - intersection_stop_lines_temp.first_attention_stop_line; + if (first_attention_stopline_valid) { + intersection_stoplines.first_attention_stopline = + intersection_stoplines_temp.first_attention_stopline; } if (occlusion_peeking_line_valid) { - intersection_stop_lines.occlusion_peeking_stop_line = - intersection_stop_lines_temp.occlusion_peeking_stop_line; + intersection_stoplines.occlusion_peeking_stopline = + intersection_stoplines_temp.occlusion_peeking_stopline; } - intersection_stop_lines.pass_judge_line = intersection_stop_lines_temp.pass_judge_line; - intersection_stop_lines.occlusion_wo_tl_pass_judge_line = - intersection_stop_lines_temp.occlusion_wo_tl_pass_judge_line; - return intersection_stop_lines; + intersection_stoplines.pass_judge_line = intersection_stoplines_temp.pass_judge_line; + intersection_stoplines.occlusion_wo_tl_pass_judge_line = + intersection_stoplines_temp.occlusion_wo_tl_pass_judge_line; + return intersection_stoplines; } std::optional getFirstPointInsidePolygon( @@ -536,30 +537,30 @@ getFirstPointInsidePolygons( std::optional generateStuckStopLine( const lanelet::CompoundPolygon3d & conflicting_area, const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stop_line_margin, + const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; const double ds = interpolated_path_info.ds; const auto & lane_interval_ip = interpolated_path_info.lane_id_interval.value(); const auto lane_interval_ip_start = std::get<0>(lane_interval_ip); - size_t stuck_stop_line_idx_ip = 0; + size_t stuck_stopline_idx_ip = 0; if (use_stuck_stopline) { - stuck_stop_line_idx_ip = lane_interval_ip_start; + stuck_stopline_idx_ip = lane_interval_ip_start; } else { - const auto stuck_stop_line_idx_ip_opt = + const auto stuck_stopline_idx_ip_opt = getFirstPointInsidePolygon(path_ip, lane_interval_ip, conflicting_area); - if (!stuck_stop_line_idx_ip_opt) { + if (!stuck_stopline_idx_ip_opt) { return std::nullopt; } - stuck_stop_line_idx_ip = stuck_stop_line_idx_ip_opt.value(); + stuck_stopline_idx_ip = stuck_stopline_idx_ip_opt.value(); } - const int stop_line_margin_idx_dist = std::ceil(stop_line_margin / ds); + const int stopline_margin_idx_dist = std::ceil(stopline_margin / ds); const int base2front_idx_dist = std::ceil(planner_data->vehicle_info_.max_longitudinal_offset_m / ds); const size_t insert_idx_ip = static_cast(std::max( - static_cast(stuck_stop_line_idx_ip) - 1 - stop_line_margin_idx_dist - base2front_idx_dist, + static_cast(stuck_stopline_idx_ip) - 1 - stopline_margin_idx_dist - base2front_idx_dist, 0)); const auto & insert_point = path_ip.points.at(insert_idx_ip).point.pose; return insertPointIndex( @@ -684,7 +685,7 @@ mergeLaneletsByTopologicalSort( IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, + const std::set & associative_ids, const double detection_area_length, const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle) { const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else"); @@ -694,8 +695,8 @@ IntersectionLanelets getObjectiveLanelets( if (const auto tl_reg_elems = assigned_lanelet.regulatoryElementsAs(); tl_reg_elems.size() != 0) { const auto tl_reg_elem = tl_reg_elems.front(); - const auto stop_line_opt = tl_reg_elem->stopLine(); - if (!!stop_line_opt) has_traffic_light = true; + const auto stopline_opt = tl_reg_elem->stopLine(); + if (!!stopline_opt) has_traffic_light = true; } // for low priority lane @@ -743,7 +744,7 @@ IntersectionLanelets getObjectiveLanelets( // final objective lanelets lanelet::ConstLanelets detection_lanelets; lanelet::ConstLanelets conflicting_ex_ego_lanelets; - // conflicting lanes is necessary to get stop_line for stuck vehicle + // conflicting lanes is necessary to get stopline for stuck vehicle for (auto && conflicting_lanelet : conflicting_lanelets) { if (!lanelet::utils::contains(ego_lanelets, conflicting_lanelet)) conflicting_ex_ego_lanelets.push_back(conflicting_lanelet); @@ -835,31 +836,31 @@ IntersectionLanelets getObjectiveLanelets( for (const auto & original_attention_lanelet_seq : original_attention_lanelet_sequences) { // NOTE: in mergeLaneletsByTopologicalSort(), sub_ids are empty checked, so it is ensured that // back() exists. - std::optional stop_line{std::nullopt}; + std::optional stopline{std::nullopt}; for (auto it = original_attention_lanelet_seq.rbegin(); it != original_attention_lanelet_seq.rend(); ++it) { const auto traffic_lights = it->regulatoryElementsAs(); for (const auto & traffic_light : traffic_lights) { - const auto stop_line_opt = traffic_light->stopLine(); - if (!stop_line_opt) continue; - stop_line = stop_line_opt.get(); + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); break; } - if (stop_line) break; + if (stopline) break; } - result.attention_stop_lines_.push_back(stop_line); + result.attention_stoplines_.push_back(stopline); } result.attention_non_preceding_ = std::move(detection_lanelets); for (unsigned i = 0; i < result.attention_non_preceding_.size(); ++i) { - std::optional stop_line = std::nullopt; + std::optional stopline = std::nullopt; const auto & ll = result.attention_non_preceding_.at(i); const auto traffic_lights = ll.regulatoryElementsAs(); for (const auto & traffic_light : traffic_lights) { - const auto stop_line_opt = traffic_light->stopLine(); - if (!stop_line_opt) continue; - stop_line = stop_line_opt.get(); + const auto stopline_opt = traffic_light->stopLine(); + if (!stopline_opt) continue; + stopline = stopline_opt.get(); } - result.attention_non_preceding_stop_lines_.push_back(stop_line); + result.attention_non_preceding_stoplines_.push_back(stopline); } result.conflicting_ = std::move(conflicting_ex_ego_lanelets); result.adjacent_ = planning_utils::getConstLaneletsFromIds(lanelet_map_ptr, associative_ids); @@ -1098,7 +1099,7 @@ std::vector generateDetectionLaneDivisions( } std::optional generateInterpolatedPath( - const int lane_id, const std::set & associative_lane_ids, + const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger) { @@ -1315,8 +1316,8 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, const bool use_upstream_velocity, const double minimum_upstream_velocity, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) @@ -1328,7 +1329,7 @@ TimeDistanceArray calcIntersectionPassingTime( // crop intersection part of the path, and set the reference velocity to intersection_velocity // for ego's ttc PathWithLaneId reference_path; - std::optional upstream_stop_line{std::nullopt}; + std::optional upstream_stopline{std::nullopt}; for (size_t i = 0; i < path.points.size() - 1; ++i) { auto reference_point = path.points.at(i); // assume backward velocity is current ego velocity @@ -1336,11 +1337,11 @@ TimeDistanceArray calcIntersectionPassingTime( reference_point.point.longitudinal_velocity_mps = current_velocity; } if ( - i > last_intersection_stop_line_candidate_idx && + i > last_intersection_stopline_candidate_idx && std::fabs(reference_point.point.longitudinal_velocity_mps) < std::numeric_limits::epsilon() && - !upstream_stop_line) { - upstream_stop_line = i; + !upstream_stopline) { + upstream_stopline = i; } if (!use_upstream_velocity) { reference_point.point.longitudinal_velocity_mps = intersection_velocity; @@ -1375,23 +1376,23 @@ TimeDistanceArray calcIntersectionPassingTime( time_distance_array.emplace_back(passing_time, dist_sum); // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so - // `last_intersection_stop_line_candidate_idx` makes no sense + // `last_intersection_stopline_candidate_idx` makes no sense const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( smoothed_reference_path.points, path.points.at(closest_idx).point.pose, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); - const std::optional upstream_stop_line_idx_opt = [&]() -> std::optional { - if (upstream_stop_line) { - const auto upstream_stop_line_point = path.points.at(upstream_stop_line.value()).point.pose; + const std::optional upstream_stopline_idx_opt = [&]() -> std::optional { + if (upstream_stopline) { + const auto upstream_stopline_point = path.points.at(upstream_stopline.value()).point.pose; return motion_utils::findFirstNearestIndexWithSoftConstraints( - smoothed_reference_path.points, upstream_stop_line_point, + smoothed_reference_path.points, upstream_stopline_point, planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold); } else { return std::nullopt; } }(); - const bool has_upstream_stopline = upstream_stop_line_idx_opt.has_value(); - const size_t upstream_stopline_ind = upstream_stop_line_idx_opt.value_or(0); + const bool has_upstream_stopline = upstream_stopline_idx_opt.has_value(); + const size_t upstream_stopline_ind = upstream_stopline_idx_opt.value_or(0); for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) { const auto & p1 = smoothed_reference_path.points.at(i); @@ -1496,7 +1497,7 @@ void IntersectionLanelets::update( } static lanelet::ConstLanelets getPrevLanelets( - const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) + const lanelet::ConstLanelets & lanelets_on_path, const std::set & associative_ids) { lanelet::ConstLanelets previous_lanelets; for (const auto & ll : lanelets_on_path) { @@ -1541,7 +1542,8 @@ lanelet::ConstLanelet generatePathLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const util::InterpolatedPathInfo & interpolated_path_info, + const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, @@ -1611,22 +1613,22 @@ std::optional generatePathLanelets( return path_lanelets; } -void TargetObject::calc_dist_to_stop_line() +void TargetObject::calc_dist_to_stopline() { - if (!attention_lanelet || !stop_line) { + if (!attention_lanelet || !stopline) { return; } const auto attention_lanelet_val = attention_lanelet.value(); const auto object_arc_coords = lanelet::utils::getArcCoordinates( {attention_lanelet_val}, object.kinematics.initial_pose_with_covariance.pose); - const auto stop_line_val = stop_line.value(); + const auto stopline_val = stopline.value(); geometry_msgs::msg::Pose stopline_center; - stopline_center.position.x = (stop_line_val.front().x() + stop_line_val.back().x()) / 2.0; - stopline_center.position.y = (stop_line_val.front().y() + stop_line_val.back().y()) / 2.0; - stopline_center.position.z = (stop_line_val.front().z() + stop_line_val.back().z()) / 2.0; + stopline_center.position.x = (stopline_val.front().x() + stopline_val.back().x()) / 2.0; + stopline_center.position.y = (stopline_val.front().y() + stopline_val.back().y()) / 2.0; + stopline_center.position.z = (stopline_val.front().z() + stopline_val.back().z()) / 2.0; const auto stopline_arc_coords = lanelet::utils::getArcCoordinates({attention_lanelet_val}, stopline_center); - dist_to_stop_line = (stopline_arc_coords.length - object_arc_coords.length); + dist_to_stopline = (stopline_arc_coords.length - object_arc_coords.length); } } // namespace util diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index 0ff35ab5c0c23..8d0e673fc931e 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -40,9 +40,10 @@ std::optional insertPoint( autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path); bool hasLaneIds( - const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set & ids); + const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, + const std::set & ids); std::optional> findLaneIdsInterval( - const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); + const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set & ids); /** * @brief get objective polygons for detection area @@ -50,7 +51,7 @@ std::optional> findLaneIdsInterval( IntersectionLanelets getObjectiveLanelets( lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path, - const std::set & associative_ids, const double detection_area_length, + const std::set & associative_ids, const double detection_area_length, const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle); /** @@ -64,7 +65,7 @@ IntersectionLanelets getObjectiveLanelets( std::optional generateStuckStopLine( const lanelet::CompoundPolygon3d & first_conflicting_area, const std::shared_ptr & planner_data, - const InterpolatedPathInfo & interpolated_path_info, const double stop_line_margin, + const InterpolatedPathInfo & interpolated_path_info, const double stopline_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional generateIntersectionStopLines( @@ -73,7 +74,7 @@ std::optional generateIntersectionStopLines( const lanelet::ConstLineString2d & first_attention_lane_centerline, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double max_accel, const double max_jerk, + const double stopline_margin, const double max_accel, const double max_jerk, const double delay_response_time, const double peeking_offset, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); @@ -117,7 +118,7 @@ std::vector generateDetectionLaneDivisions( const double curvature_threshold, const double curvature_calculation_ds); std::optional generateInterpolatedPath( - const int lane_id, const std::set & associative_lane_ids, + const lanelet::Id lane_id, const std::set & associative_lane_ids, const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds, const rclcpp::Logger logger); @@ -150,8 +151,8 @@ void cutPredictPathWithDuration( TimeDistanceArray calcIntersectionPassingTime( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const std::shared_ptr & planner_data, const lanelet::Id lane_id, - const std::set & associative_ids, const size_t closest_idx, - const size_t last_intersection_stop_line_candidate_idx, const double time_delay, + const std::set & associative_ids, const size_t closest_idx, + const size_t last_intersection_stopline_candidate_idx, const double time_delay, const double intersection_velocity, const double minimum_ego_velocity, const bool use_upstream_velocity, const double minimum_upstream_velocity, tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array); @@ -166,7 +167,8 @@ lanelet::ConstLanelet generatePathLanelet( std::optional generatePathLanelets( const lanelet::ConstLanelets & lanelets_on_path, - const util::InterpolatedPathInfo & interpolated_path_info, const std::set & associative_ids, + const util::InterpolatedPathInfo & interpolated_path_info, + const std::set & associative_ids, const lanelet::CompoundPolygon3d & first_conflicting_area, const std::vector & conflicting_areas, const std::optional & first_attention_area, diff --git a/planning/behavior_velocity_intersection_module/src/util_type.hpp b/planning/behavior_velocity_intersection_module/src/util_type.hpp index d1ee4c2f79410..3c7ba3041b0bd 100644 --- a/planning/behavior_velocity_intersection_module/src/util_type.hpp +++ b/planning/behavior_velocity_intersection_module/src/util_type.hpp @@ -64,8 +64,8 @@ struct InterpolatedPathInfo { autoware_auto_planning_msgs::msg::PathWithLaneId path; double ds{0.0}; - int lane_id{0}; - std::set associative_lane_ids{}; + lanelet::Id lane_id{0}; + std::set associative_lane_ids{}; std::optional> lane_id_interval{std::nullopt}; }; @@ -79,9 +79,9 @@ struct IntersectionLanelets { return is_prioritized_ ? attention_non_preceding_ : attention_; } - const std::vector> & attention_stop_lines() const + const std::vector> & attention_stoplines() const { - return is_prioritized_ ? attention_non_preceding_stop_lines_ : attention_stop_lines_; + return is_prioritized_ ? attention_non_preceding_stoplines_ : attention_stoplines_; } const lanelet::ConstLanelets & conflicting() const { return conflicting_; } const lanelet::ConstLanelets & adjacent() const { return adjacent_; } @@ -121,11 +121,11 @@ struct IntersectionLanelets lanelet::ConstLanelets attention_; // topologically merged lanelets std::vector> - attention_stop_lines_; // the stop lines for each attention_ lanelets + attention_stoplines_; // the stop lines for each attention_ lanelets lanelet::ConstLanelets attention_non_preceding_; std::vector> - attention_non_preceding_stop_lines_; // the stop lines for each attention_non_preceding_ - // lanelets + attention_non_preceding_stoplines_; // the stop lines for each attention_non_preceding_ + // lanelets lanelet::ConstLanelets conflicting_; lanelet::ConstLanelets adjacent_; lanelet::ConstLanelets occlusion_attention_; // topologically merged lanelets @@ -151,13 +151,13 @@ struct IntersectionStopLines // NOTE: for baselink size_t closest_idx{0}; // NOTE: null if path does not conflict with first_conflicting_area - std::optional stuck_stop_line{std::nullopt}; - // NOTE: null if path is over map stop_line OR its value is calculated negative - std::optional default_stop_line{std::nullopt}; + std::optional stuck_stopline{std::nullopt}; + // NOTE: null if path is over map stopline OR its value is calculated negative + std::optional default_stopline{std::nullopt}; // NOTE: null if the index is calculated negative - std::optional first_attention_stop_line{std::nullopt}; + std::optional first_attention_stopline{std::nullopt}; // NOTE: null if footprints do not change from outside to inside of detection area - std::optional occlusion_peeking_stop_line{std::nullopt}; + std::optional occlusion_peeking_stopline{std::nullopt}; // if the value is calculated negative, its value is 0 size_t pass_judge_line{0}; size_t occlusion_wo_tl_pass_judge_line{0}; @@ -175,16 +175,17 @@ struct PathLanelets lanelet::ConstLanelets all; lanelet::ConstLanelets conflicting_interval_and_remaining; // the left/right-most interval of path conflicting with - // conflicting lanelets plus the next lane part of the path + // conflicting lanelets plus the next lane part of the + // path }; struct TargetObject { autoware_auto_perception_msgs::msg::PredictedObject object; std::optional attention_lanelet{std::nullopt}; - std::optional stop_line{std::nullopt}; - std::optional dist_to_stop_line{std::nullopt}; - void calc_dist_to_stop_line(); + std::optional stopline{std::nullopt}; + std::optional dist_to_stopline{std::nullopt}; + void calc_dist_to_stopline(); }; struct TargetObjects diff --git a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp index 0d0e7cc6cadba..62e67f6346806 100644 --- a/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp +++ b/planning/behavior_velocity_no_drivable_lane_module/src/scene.cpp @@ -33,7 +33,7 @@ NoDrivableLaneModule::NoDrivableLaneModule( planner_param_(planner_param), state_(State::INIT) { - velocity_factor_.init(VelocityFactor::NO_DRIVABLE_LANE); + velocity_factor_.init(PlanningBehavior::NO_DRIVABLE_LANE); } bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) @@ -57,7 +57,7 @@ bool NoDrivableLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason if (path_no_drivable_lane_polygon_intersection.first_intersection_point) { first_intersection_point = - path_no_drivable_lane_polygon_intersection.first_intersection_point.get(); + path_no_drivable_lane_polygon_intersection.first_intersection_point.value(); distance_ego_first_intersection = motion_utils::calcSignedArcLength( path->points, planner_data_->current_odometry->pose.position, first_intersection_point); distance_ego_first_intersection -= planner_data_->vehicle_info_.max_longitudinal_offset_m; @@ -141,7 +141,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR geometry_msgs::msg::Point target_point; if (op_target_point) { - target_point = op_target_point.get(); + target_point = op_target_point.value(); } const auto target_segment_idx = motion_utils::findNearestSegmentIndex(path->points, target_point); @@ -150,7 +150,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR motion_utils::insertTargetPoint(target_segment_idx, target_point, path->points, 5e-2); size_t target_point_idx; if (op_target_point_idx) { - target_point_idx = op_target_point_idx.get(); + target_point_idx = op_target_point_idx.value(); } geometry_msgs::msg::Point stop_point = @@ -162,7 +162,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR // Get stop point and stop factor { tier4_planning_msgs::msg::StopFactor stop_factor; - const auto & stop_pose = op_stop_pose.get(); + const auto & stop_pose = op_stop_pose.value(); stop_factor.stop_pose = stop_pose; stop_factor.stop_factor_points.push_back(stop_point); planning_utils::appendStopReason(stop_factor, stop_reason); @@ -172,7 +172,7 @@ void NoDrivableLaneModule::handle_approaching_state(PathWithLaneId * path, StopR const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); - debug_data_.stop_pose = virtual_wall_pose.get(); + debug_data_.stop_pose = virtual_wall_pose.value(); } const size_t current_seg_idx = findEgoSegmentIndex(path->points); @@ -220,13 +220,12 @@ void NoDrivableLaneModule::handle_inside_no_drivable_lane_state( stop_factor.stop_factor_points.push_back(current_point); planning_utils::appendStopReason(stop_factor, stop_reason); velocity_factor_.set( - path->points, planner_data_->current_odometry->pose, stop_pose, - VelocityFactor::NO_DRIVABLE_LANE); + path->points, planner_data_->current_odometry->pose, stop_pose, VelocityFactor::APPROACHING); const auto & virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); - debug_data_.stop_pose = virtual_wall_pose.get(); + debug_data_.stop_pose = virtual_wall_pose.value(); } // Move to stopped state if stopped @@ -249,7 +248,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso } SegmentIndexWithPose ego_pos_on_path; - ego_pos_on_path.pose = stopped_pose.get(); + ego_pos_on_path.pose = stopped_pose.value(); ego_pos_on_path.index = findEgoSegmentIndex(path->points); // Insert stop pose @@ -268,7 +267,7 @@ void NoDrivableLaneModule::handle_stopped_state(PathWithLaneId * path, StopReaso const auto virtual_wall_pose = motion_utils::calcLongitudinalOffsetPose( path->points, stop_pose.position, debug_data_.base_link2front); - debug_data_.stop_pose = virtual_wall_pose.get(); + debug_data_.stop_pose = virtual_wall_pose.value(); } } diff --git a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp index ceef50094e374..47d4985ae643e 100644 --- a/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp +++ b/planning/behavior_velocity_no_stopping_area_module/src/scene_no_stopping_area.cpp @@ -50,7 +50,7 @@ NoStoppingAreaModule::NoStoppingAreaModule( no_stopping_area_reg_elem_(no_stopping_area_reg_elem), planner_param_(planner_param) { - velocity_factor_.init(VelocityFactor::NO_STOPPING_AREA); + velocity_factor_.init(PlanningBehavior::NO_STOPPING_AREA); state_machine_.setState(StateMachine::State::GO); state_machine_.setMarginTime(planner_param_.state_clear_time); } @@ -298,7 +298,7 @@ Polygon2d NoStoppingAreaModule::generateEgoNoStoppingAreaLanePolygon( logger_, *clock_, 1000 /* ms */, "motion_utils::findNearestIndex fail"); return ego_area; } - const size_t closest_idx = closest_idx_opt.get(); + const size_t closest_idx = closest_idx_opt.value(); const int num_ignore_nearest = 1; // Do not consider nearest lane polygon size_t ego_area_start_idx = closest_idx + num_ignore_nearest; diff --git a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp index ef181ee7613a0..96f497ca47afa 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/risk_predictive_braking.cpp @@ -64,7 +64,7 @@ void applySafeVelocityConsideringPossibleCollision( const auto & pose = possible_collision.collision_with_margin.pose; const auto & decel_pose = planning_utils::insertDecelPoint(pose.position, *inout_path, safe_velocity); - if (decel_pose) debug_poses.push_back(decel_pose.get()); + if (decel_pose) debug_poses.push_back(decel_pose.value()); } } diff --git a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp index b7278aff29902..8ac3fe26c1c33 100644 --- a/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp +++ b/planning/behavior_velocity_occlusion_spot_module/src/scene_occlusion_spot.cpp @@ -66,7 +66,7 @@ OcclusionSpotModule::OcclusionSpotModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), param_(planner_param) { - velocity_factor_.init(VelocityFactor::UNKNOWN); + velocity_factor_.init(PlanningBehavior::UNKNOWN); if (param_.detection_method == utils::DETECTION_METHOD::OCCUPANCY_GRID) { debug_data_.detection_type = "occupancy"; @@ -112,7 +112,7 @@ bool OcclusionSpotModule::modifyPathVelocity( const geometry_msgs::msg::Point start_point = path_interpolated.points.at(0).point.pose.position; const auto ego_segment_idx = motion_utils::findNearestSegmentIndex( path_interpolated.points, ego_pose, param_.dist_thr, param_.angle_thr); - if (ego_segment_idx == boost::none) return true; + if (!ego_segment_idx) return true; const size_t start_point_segment_idx = motion_utils::findNearestSegmentIndex(path_interpolated.points, start_point); const auto offset = motion_utils::calcSignedArcLength( diff --git a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp index d739c6440ae75..1f6a332e27e8d 100644 --- a/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp +++ b/planning/behavior_velocity_out_of_lane_module/src/scene_out_of_lane.cpp @@ -52,7 +52,7 @@ OutOfLaneModule::OutOfLaneModule( const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param)) { - velocity_factor_.init(VelocityFactor::UNKNOWN); + velocity_factor_.init(PlanningBehavior::UNKNOWN); } bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) diff --git a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml index 46472619d5fb0..22220ccd182a1 100644 --- a/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml +++ b/planning/behavior_velocity_planner/launch/behavior_velocity_planner.launch.xml @@ -27,6 +27,7 @@ + @@ -69,5 +70,7 @@ + + diff --git a/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json b/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json new file mode 100644 index 0000000000000..a119139a13a20 --- /dev/null +++ b/planning/behavior_velocity_planner/schema/behavior_velocity_planner.schema.json @@ -0,0 +1,77 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for Behavior Velocity Planner", + "type": "object", + "definitions": { + "behavior_velocity_planner": { + "type": "object", + "properties": { + "forward_path_length": { + "type": "number", + "default": "1000.0", + "description": "forward path" + }, + "backward_path_length": { + "type": "number", + "default": "5.0", + "description": "backward path" + }, + "max_accel": { + "type": "number", + "default": "-2.8", + "description": "(to be a global parameter) max acceleration of the vehicle" + }, + "system_delay": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time until output control command" + }, + "delay_response_time": { + "type": "number", + "default": "0.5", + "description": "(to be a global parameter) delay time of the vehicle's response to control commands" + }, + "stop_line_extend_length": { + "type": "number", + "default": "5.0", + "description": "extend length of stop line" + }, + "max_jerk": { + "type": "number", + "default": "-5.0", + "description": "max jerk of the vehicle" + }, + "is_publish_debug_path": { + "type": "boolean", + "default": "false", + "description": "is publish debug path?" + } + }, + "required": [ + "forward_path_length", + "backward_path_length", + "max_accel", + "system_delay", + "delay_response_time", + "stop_line_extend_length", + "max_jerk", + "is_publish_debug_path" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/behavior_velocity_planner" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index ec28656d8f866..7c0036b7812fa 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -136,16 +136,14 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio debug_viz_pub_ = this->create_publisher("~/debug/path", 1); // Parameters - forward_path_length_ = this->declare_parameter("forward_path_length"); - backward_path_length_ = this->declare_parameter("backward_path_length"); - planner_data_.stop_line_extend_length = - this->declare_parameter("stop_line_extend_length"); + forward_path_length_ = declare_parameter("forward_path_length"); + backward_path_length_ = declare_parameter("backward_path_length"); + planner_data_.stop_line_extend_length = declare_parameter("stop_line_extend_length"); // nearest search planner_data_.ego_nearest_dist_threshold = - this->declare_parameter("ego_nearest_dist_threshold"); - planner_data_.ego_nearest_yaw_threshold = - this->declare_parameter("ego_nearest_yaw_threshold"); + declare_parameter("ego_nearest_dist_threshold"); + planner_data_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); // Initialize PlannerManager for (const auto & name : declare_parameter>("launch_modules")) { @@ -386,7 +384,7 @@ autoware_auto_planning_msgs::msg::Path BehaviorVelocityPlannerNode::generatePath // TODO(someone): support backward path const auto is_driving_forward = motion_utils::isDrivingForward(input_path_msg->points); - is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; + is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; if (!is_driving_forward_) { RCLCPP_WARN_THROTTLE( get_logger(), *get_clock(), 3000, diff --git a/planning/behavior_velocity_planner/src/planner_manager.cpp b/planning/behavior_velocity_planner/src/planner_manager.cpp index e1c801a27dca3..b4cb87d0b5258 100644 --- a/planning/behavior_velocity_planner/src/planner_manager.cpp +++ b/planning/behavior_velocity_planner/src/planner_manager.cpp @@ -108,11 +108,11 @@ autoware_auto_planning_msgs::msg::PathWithLaneId BehaviorVelocityPlannerManager: for (const auto & plugin : scene_manager_plugins_) { plugin->updateSceneModuleInstances(planner_data, input_path_msg); plugin->plan(&output_path_msg); - boost::optional firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); + const auto firstStopPathPointIndex = plugin->getFirstStopPathPointIndex(); if (firstStopPathPointIndex) { - if (firstStopPathPointIndex.get() < first_stop_path_point_index) { - first_stop_path_point_index = firstStopPathPointIndex.get(); + if (firstStopPathPointIndex.value() < first_stop_path_point_index) { + first_stop_path_point_index = firstStopPathPointIndex.value(); stop_reason_msg = plugin->getModuleName(); } } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp index 620fc61ec5360..2ea9f6ed2648b 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/planner_data.hpp @@ -35,8 +35,6 @@ #include #include -#include - #include #include @@ -44,6 +42,7 @@ #include #include #include +#include #include namespace behavior_velocity_planner @@ -78,7 +77,7 @@ struct PlannerData // other internal data std::map traffic_light_id_map; - boost::optional external_velocity_limit; + std::optional external_velocity_limit; tier4_v2x_msgs::msg::VirtualTrafficLightStateArray::ConstSharedPtr virtual_traffic_light_states; // velocity smoother diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp index e5606d7d10dff..624da6b170d06 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_interface.hpp @@ -34,7 +34,7 @@ class PluginInterface virtual void updateSceneModuleInstances( const std::shared_ptr & planner_data, const autoware_auto_planning_msgs::msg::PathWithLaneId & path) = 0; - virtual boost::optional getFirstStopPathPointIndex() = 0; + virtual std::optional getFirstStopPathPointIndex() = 0; virtual const char * getModuleName() = 0; }; diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp index 623649f82bf24..7aa3e6bfef9ab 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/plugin_wrapper.hpp @@ -18,6 +18,7 @@ #include #include +#include namespace behavior_velocity_planner { @@ -37,7 +38,7 @@ class PluginWrapper : public PluginInterface { scene_manager_->updateSceneModuleInstances(planner_data, path); } - boost::optional getFirstStopPathPointIndex() override + std::optional getFirstStopPathPointIndex() override { return scene_manager_->getFirstStopPathPointIndex(); } diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp index 90e78215cae2d..ced2e267cc025 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp @@ -73,20 +73,21 @@ class SceneModuleInterface planner_data_ = planner_data; } - boost::optional getInfrastructureCommand() + std::optional getInfrastructureCommand() { return infrastructure_command_; } void setInfrastructureCommand( - const boost::optional & command) + const std::optional & command) { infrastructure_command_ = command; } - boost::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } void setActivation(const bool activated) { activated_ = activated; } + void setRTCEnabled(const bool enable_rtc) { rtc_enabled_ = enable_rtc; } bool isActivated() const { return activated_; } bool isSafe() const { return safe_; } double getDistance() const { return distance_; } @@ -98,16 +99,24 @@ class SceneModuleInterface const int64_t module_id_; bool activated_; bool safe_; + bool rtc_enabled_; double distance_; rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; std::shared_ptr planner_data_; - boost::optional infrastructure_command_; - boost::optional first_stop_path_point_index_; + std::optional infrastructure_command_; + std::optional first_stop_path_point_index_; VelocityFactorInterface velocity_factor_; - void setSafe(const bool safe) { safe_ = safe; } + void setSafe(const bool safe) + { + safe_ = safe; + if (!rtc_enabled_) { + syncActivation(); + } + } void setDistance(const double distance) { distance_ = distance; } + void syncActivation() { setActivation(isSafe()); } size_t findEgoSegmentIndex( const std::vector & points) const; @@ -122,7 +131,7 @@ class SceneModuleManagerInterface virtual const char * getModuleName() = 0; - boost::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } + std::optional getFirstStopPathPointIndex() { return first_stop_path_point_index_; } void updateSceneModuleInstances( const std::shared_ptr & planner_data, @@ -161,7 +170,7 @@ class SceneModuleManagerInterface std::shared_ptr planner_data_; motion_utils::VirtualWallMarkerCreator virtual_wall_marker_creator_; - boost::optional first_stop_path_point_index_; + std::optional first_stop_path_point_index_; rclcpp::Node & node_; rclcpp::Clock::SharedPtr clock_; // Debug diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp index 63d7c08b945fd..98cf5114fb0e7 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/arc_lane_util.hpp @@ -20,20 +20,15 @@ #include -#include - #include -#include +#include #include -#include #define EIGEN_MPL2_ONLY #include namespace behavior_velocity_planner { -namespace bg = boost::geometry; - namespace { geometry_msgs::msg::Point convertToGeomPoint(const tier4_autoware_utils::Point2d & p) @@ -59,12 +54,12 @@ double calcSignedDistance( const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Point & p2); // calculate one collision point between the line (from p1 to p2) and the line (from p3 to p4) -boost::optional checkCollision( +std::optional checkCollision( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4); template -boost::optional findCollisionSegment( +std::optional findCollisionSegment( const T & path, const geometry_msgs::msg::Point & stop_line_p1, const geometry_msgs::msg::Point & stop_line_p2, const size_t target_lane_id) { @@ -88,7 +83,7 @@ boost::optional findCollisionSegment( const auto collision_point = checkCollision(p1, p2, stop_line_p1, stop_line_p2); if (collision_point) { - return std::make_pair(i, collision_point.get()); + return std::make_pair(i, collision_point.value()); } } @@ -96,7 +91,7 @@ boost::optional findCollisionSegment( } template -boost::optional findCollisionSegment( +std::optional findCollisionSegment( const T & path, const LineString2d & stop_line, const size_t target_lane_id) { const auto stop_line_p1 = convertToGeomPoint(stop_line.at(0)); @@ -106,7 +101,7 @@ boost::optional findCollisionSegment( } template -boost::optional findForwardOffsetSegment( +std::optional findForwardOffsetSegment( const T & path, const size_t base_idx, const double offset_length) { double sum_length = 0.0; @@ -124,7 +119,7 @@ boost::optional findForwardOffsetSegment( } template -boost::optional findBackwardOffsetSegment( +std::optional findBackwardOffsetSegment( const T & path, const size_t base_idx, const double offset_length) { double sum_length = 0.0; @@ -144,7 +139,7 @@ boost::optional findBackwardOffsetSegment( } template -boost::optional findOffsetSegment( +std::optional findOffsetSegment( const T & path, const PathIndexWithPoint & collision_segment, const double offset_length) { const size_t collision_idx = collision_segment.first; @@ -163,7 +158,7 @@ boost::optional findOffsetSegment( tier4_autoware_utils::calcDistance2d(path.points.at(collision_idx + 1), collision_point)); } -boost::optional findOffsetSegment( +std::optional findOffsetSegment( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t index, const double offset); @@ -196,7 +191,7 @@ geometry_msgs::msg::Pose calcTargetPose(const T & path, const PathIndexWithOffse return target_pose; } -boost::optional createTargetPoint( +std::optional createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const size_t lane_id, const double margin, const double vehicle_offset); diff --git a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp index 86273b1e35e1d..e042338c53485 100644 --- a/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp +++ b/planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp @@ -27,9 +27,8 @@ #include #include -#include -#include #include +#include #include #include #include @@ -133,7 +132,7 @@ std::vector concatVector(const std::vector & vec1, const std::vector & return concat_vec; } -boost::optional getNearestLaneId( +std::optional getNearestLaneId( const PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map, const geometry_msgs::msg::Pose & current_pose); @@ -198,7 +197,7 @@ std::set getLaneletIdSetOnPath( return id_set; } -boost::optional insertDecelPoint( +std::optional insertDecelPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output, const float target_velocity); @@ -215,22 +214,22 @@ bool isOverLine( const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, const double offset = 0.0); -boost::optional insertStopPoint( +std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, PathWithLaneId & output); -boost::optional insertStopPoint( +std::optional insertStopPoint( const geometry_msgs::msg::Point & stop_point, const size_t stop_seg_idx, PathWithLaneId & output); /* @brief return 'associative' lanes in the intersection. 'associative' means that a lane shares same or lane-changeable parent lanes with `lane` and has same turn_direction value. */ -std::set getAssociativeIntersectionLanelets( +std::set getAssociativeIntersectionLanelets( lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map, const lanelet::routing::RoutingGraphPtr routing_graph); template