diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index 12ad752b3ef29..89bcbce9e5656 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -1,7 +1,7 @@ ### Copied from .github/CODEOWNERS-manual ### ### Automatically generated from package.xml ### -common/autoware_ad_api_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_auto_geometry/** satoshi.ota@tier4.jp common/autoware_auto_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp @@ -9,9 +9,9 @@ common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wak common/autoware_point_types/** taichi.higashide@tier4.jp common/autoware_testing/** adam.dabrowski@robotec.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/bag_time_manager_rviz_plugin/** taiki.tanaka@tier4.jp -common/component_interface_specs/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp -common/component_interface_tools/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -common/component_interface_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_specs/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp +common/component_interface_tools/** isamu.takagi@tier4.jp +common/component_interface_utils/** isamu.takagi@tier4.jp yukihiro.saito@tier4.jp common/cuda_utils/** daisuke.nishimatsu@tier4.jp manato.hirabayashi@tier4.jp common/fake_test_node/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/geography_utils/** koji.minoda@tier4.jp @@ -24,7 +24,7 @@ common/kalman_filter/** koji.minoda@tier4.jp takeshi.ishita@tier4.jp yukihiro.sa 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 +common/path_distance_calculator/** isamu.takagi@tier4.jp common/perception_utils/** satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp yusuke.muramatsu@tier4.jp common/polar_grid/** yukihiro.saito@tier4.jp common/qp_interface/** fumiya.watanabe@tier4.jp maxime.clement@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp @@ -32,16 +32,16 @@ common/rtc_manager_rviz_plugin/** taiki.tanaka@tier4.jp tomoya.kimura@tier4.jp common/signal_processing/** ali.boyali@tier4.jp fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp common/tensorrt_common/** daisuke.nishimatsu@tier4.jp dan.umeda@tier4.jp manato.hirabayashi@tier4.jp common/tier4_adapi_rviz_plugin/** hiroki.ota@tier4.jp isamu.takagi@tier4.jp kosuke.takeuchi@tier4.jp -common/tier4_api_utils/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp +common/tier4_api_utils/** isamu.takagi@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 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_datetime_rviz_plugin/** isamu.takagi@tier4.jp common/tier4_debug_rviz_plugin/** takayuki.murooka@tier4.jp 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_localization_rviz_plugin/** isamu.takagi@tier4.jp takamasa.horibe@tier4.jp common/tier4_logging_level_configure_rviz_plugin/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takamasa.horibe@tier4.jp common/tier4_perception_rviz_plugin/** yukihiro.saito@tier4.jp common/tier4_planning_rviz_plugin/** takayuki.murooka@tier4.jp yukihiro.saito@tier4.jp @@ -54,7 +54,7 @@ common/tier4_traffic_light_rviz_plugin/** satoshi.ota@tier4.jp common/tier4_vehicle_rviz_plugin/** yukihiro.saito@tier4.jp common/time_utils/** christopherj.ho@gmail.com shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp 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/traffic_light_utils/** mingyu.li@tier4.jp satoshi.ota@tier4.jp shunsuke.miura@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 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 @@ -76,9 +76,9 @@ evaluator/diagnostic_converter/** kyoichi.sugahara@tier4.jp maxime.clement@tier4 evaluator/kinematic_evaluator/** dominik.jargot@robotec.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp evaluator/localization_evaluator/** dominik.jargot@robotec.ai koji.minoda@tier4.jp evaluator/planning_evaluator/** kyoichi.sugahara@tier4.jp maxime.clement@tier4.jp -launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp ryohsuke.mitsudome@tier4.jp +launch/tier4_autoware_api_launch/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp launch/tier4_control_launch/** takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp -launch/tier4_localization_launch/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp yamato.ando@tier4.jp +launch/tier4_localization_launch/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp launch/tier4_map_launch/** kento.yabuuchi.2@tier4.jp 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 zhe.shen@tier4.jp zulfaqar.azmi@tier4.jp @@ -86,29 +86,29 @@ 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/** 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_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 -localization/yabloc/yabloc_common/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_image_processing/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_monitor/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_particle_filter/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -localization/yabloc/yabloc_pose_initializer/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp -map/map_height_fitter/** isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp -map/map_projection_loader/** kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp -map/map_tf_generator/** kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp yamato.ando@tier4.jp +localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp +localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/landmark_based_localizer/landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_error_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/localization_util/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/ndt_scan_matcher/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/tree_structured_parzen_estimator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_monitor/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_particle_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +localization/yabloc/yabloc_pose_initializer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_height_fitter/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryohsuke.mitsudome@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp +map/map_projection_loader/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp +map/map_tf_generator/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp map/util/lanelet2_map_preprocessor/** ryohsuke.mitsudome@tier4.jp perception/bytetrack/** manato.hirabayashi@tier4.jp perception/cluster_merger/** dai.nguyen@tier4.jp shunsuke.miura@tier4.jp yukihiro.saito@tier4.jp @@ -156,6 +156,7 @@ perception/traffic_light_ssd_fine_detector/** daisuke.nishimatsu@tier4.jp perception/traffic_light_visualization/** yukihiro.saito@tier4.jp planning/behavior_path_avoidance_by_lane_change_module/** fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_avoidance_module/** fumiya.watanabe@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_path_dynamic_avoidance_module/** kosuke.takeuchi@tier4.jp satoshi.ota@tier4.jp takayuki.murooka@tier4.jp yuki.takagi@tier4.jp planning/behavior_path_external_request_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp planning/behavior_path_goal_planner_module/** kosuke.takeuchi@tier4.jp kyoichi.sugahara@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp planning/behavior_path_lane_change_module/** fumiya.watanabe@tier4.jp kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomohito.ando@tier4.jp tomoya.kimura@tier4.jp zulfaqar.azmi@tier4.jp @@ -166,6 +167,7 @@ planning/behavior_path_start_planner_module/** kosuke.takeuchi@tier4.jp kyoichi. 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 mamoru.sobue@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp yuki.takagi@tier4.jp planning/behavior_velocity_detection_area_module/** kyoichi.sugahara@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp +planning/behavior_velocity_dynamic_obstacle_stop_module/** mamoru.sobue@tier4.jp maxime.clement@tier4.jp planning/behavior_velocity_intersection_module/** kyoichi.sugahara@tier4.jp mamoru.sobue@tier4.jp shumpei.wakabayashi@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_drivable_lane_module/** ahmed.ebrahim@leodrive.ai fumiya.watanabe@tier4.jp satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp takamasa.horibe@tier4.jp takayuki.murooka@tier4.jp tomoya.kimura@tier4.jp planning/behavior_velocity_no_stopping_area_module/** kosuke.takeuchi@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp @@ -223,11 +225,11 @@ simulator/fault_injection/** keisuke.shima@tier4.jp simulator/simple_planning_simulator/** takamasa.horibe@tier4.jp tomoya.kimura@tier4.jp system/autoware_auto_msgs_adapter/** isamu.takagi@tier4.jp mfc@leodrive.ai system/bluetooth_monitor/** fumihito.ito@tier4.jp -system/component_state_monitor/** isamu.takagi@tier4.jp kahhooi.tan@tier4.jp makoto.yabuta@tier4.jp -system/default_ad_api/** 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_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/component_state_monitor/** isamu.takagi@tier4.jp +system/default_ad_api/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_adaptors/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/ad_api_visualizers/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp +system/default_ad_api_helpers/automatic_pose_initializer/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp yukihiro.saito@tier4.jp system/diagnostic_graph_aggregator/** isamu.takagi@tier4.jp system/dummy_diag_publisher/** fumihito.ito@tier4.jp tetsuhiro.kawaguchi@tier4.jp system/dummy_infrastructure/** ryohsuke.mitsudome@tier4.jp 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 10d1ac034d457..d269144067e0e 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 @@ -267,6 +267,8 @@ + + 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 c227298c932d9..4838187ef8fbe 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 @@ -296,6 +296,8 @@ + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 87616b9ccb122..4e9f0daafa736 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -34,6 +34,9 @@ + + + diff --git a/perception/radar_tracks_msgs_converter/README.md b/perception/radar_tracks_msgs_converter/README.md index fb8f117c82245..fa08eb08f521a 100644 --- a/perception/radar_tracks_msgs_converter/README.md +++ b/perception/radar_tracks_msgs_converter/README.md @@ -23,7 +23,7 @@ This package converts from [radar_msgs/msg/RadarTracks](https://github.com/ros-p - `new_frame_id` (string): The header frame of the output topic. - Default parameter is "base_link" - `use_twist_compensation` (bool): If the parameter is true, then the twist of the output objects' topic is compensated by ego vehicle motion. - - Default parameter is "false" + - Default parameter is "true" - `use_twist_yaw_compensation` (bool): If the parameter is true, then the ego motion compensation will also consider yaw motion of the ego vehicle. - Default parameter is "false" - `static_object_speed_threshold` (float): Specify the threshold for static object speed which determines the flag `is_stationary` [m/s]. diff --git a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml index a6a3f394b1f40..cc2bb80c6f4f7 100644 --- a/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml +++ b/perception/radar_tracks_msgs_converter/launch/radar_tracks_msgs_converter.launch.xml @@ -4,7 +4,7 @@ - + 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 9b3fbedc37203..3eb5eca5954fb 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -784,6 +784,7 @@ void BehaviorPathPlannerNode::onTrafficSignals(const TrafficSignalArray::ConstSh { std::lock_guard lock(mutex_pd_); + planner_data_->traffic_light_id_map.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; diff --git a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml index f9ddd3ce61099..3c22d1fe65db5 100644 --- a/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_blind_spot_module/config/blind_spot.param.yaml @@ -8,4 +8,5 @@ max_future_movement_time: 10.0 # [second] threshold_yaw_diff: 0.523 # [rad] adjacent_extend_width: 1.5 # [m] + opposite_adjacent_extend_width: 1.5 # [m] enable_rtc: false # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval diff --git a/planning/behavior_velocity_blind_spot_module/src/debug.cpp b/planning/behavior_velocity_blind_spot_module/src/debug.cpp index 7da494bfd5231..5cc90afb0043d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/debug.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/debug.cpp @@ -145,14 +145,12 @@ visualization_msgs::msg::MarkerArray BlindSpotModule::createDebugMarkerArray() appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.conflict_areas_for_blind_spot, "conflict_area_for_blind_spot", module_id_, 0.0, - 0.5, 0.5), + debug_data_.conflict_areas, "conflict_area", module_id_, 0.0, 0.5, 0.5), &debug_marker_array, now); appendMarkerArray( createLaneletPolygonsMarkerArray( - debug_data_.detection_areas_for_blind_spot, "detection_area_for_blind_spot", module_id_, 0.5, - 0.0, 0.0), + debug_data_.detection_areas, "detection_area", module_id_, 0.5, 0.0, 0.0), &debug_marker_array, now); appendMarkerArray( diff --git a/planning/behavior_velocity_blind_spot_module/src/manager.cpp b/planning/behavior_velocity_blind_spot_module/src/manager.cpp index d64f1ebc39a51..09d1c5a7c3270 100644 --- a/planning/behavior_velocity_blind_spot_module/src/manager.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/manager.cpp @@ -48,6 +48,8 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".threshold_yaw_diff"); planner_param_.adjacent_extend_width = getOrDeclareParameter(node, ns + ".adjacent_extend_width"); + planner_param_.opposite_adjacent_extend_width = + getOrDeclareParameter(node, ns + ".opposite_adjacent_extend_width"); } void BlindSpotModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.cpp b/planning/behavior_velocity_blind_spot_module/src/scene.cpp index 62a5e06674d4e..efbff7e2af51d 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.cpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.cpp @@ -105,16 +105,18 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto const auto routing_graph_ptr = planner_data_->route_handler_->getRoutingGraphPtr(); /* set stop-line and stop-judgement-line for base_link */ - int stop_line_idx = -1; const auto straight_lanelets = getStraightLanelets(lanelet_map_ptr, routing_graph_ptr, lane_id_); - if (!generateStopLine(straight_lanelets, path, &stop_line_idx)) { + const auto stoplines_idx_opt = generateStopLine(straight_lanelets, path); + if (!stoplines_idx_opt) { RCLCPP_WARN_SKIPFIRST_THROTTLE( logger_, *clock_, 1000 /* ms */, "[BlindSpotModule::run] setStopLineIdx fail"); *path = input_path; // reset path return false; } - if (stop_line_idx <= 0) { + const auto [default_stopline_idx, critical_stopline_idx] = stoplines_idx_opt.value(); + + if (default_stopline_idx <= 0) { RCLCPP_DEBUG( logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); *path = input_path; // reset path @@ -133,6 +135,8 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto return false; } const size_t closest_idx = closest_idx_opt.value(); + const auto stop_line_idx = + closest_idx > default_stopline_idx ? critical_stopline_idx : default_stopline_idx; /* set judge line dist */ const double current_vel = planner_data_->current_velocity->twist.linear.x; @@ -156,9 +160,6 @@ bool BlindSpotModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto debug_data_.virtual_wall_pose = stop_line_pose; const auto stop_pose = path->points.at(stop_line_idx).point.pose; debug_data_.stop_point_pose = stop_pose; - auto offset_pose = motion_utils::calcLongitudinalOffsetPose( - path->points, stop_pose.position, -pass_judge_line_dist); - if (offset_pose) debug_data_.judge_point_pose = *offset_pose; /* if current_state = GO, and current_pose is over judge_line, ignore planning. */ if (planner_param_.use_pass_judge_line) { @@ -235,69 +236,41 @@ std::optional BlindSpotModule::getFirstPointConflictingLanelets( } } -bool BlindSpotModule::generateStopLine( +std::optional> BlindSpotModule::generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const { /* set parameters */ constexpr double interval = 0.2; const int margin_idx_dist = std::ceil(planner_param_.stop_line_margin / interval); - const int base2front_idx_dist = - std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); + // const int base2front_idx_dist = + // std::ceil(planner_data_->vehicle_info_.max_longitudinal_offset_m / interval); /* spline interpolation */ autoware_auto_planning_msgs::msg::PathWithLaneId path_ip; if (!splineInterpolate(*path, interval, path_ip, logger_)) { - return false; + return std::nullopt; } /* generate stop point */ - int stop_idx_ip = 0; // stop point index for interpolated path. + size_t stop_idx_default_ip = 0; // stop point index for interpolated path. + size_t stop_idx_critical_ip = 0; if (straight_lanelets.size() > 0) { std::optional first_idx_conflicting_lane_opt = getFirstPointConflictingLanelets(path_ip, straight_lanelets); if (!first_idx_conflicting_lane_opt) { RCLCPP_DEBUG(logger_, "No conflicting line found."); - return false; - } - stop_idx_ip = std::max( - first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist - base2front_idx_dist, 0); - } else { - std::optional intersection_enter_point_opt = - getStartPointFromLaneLet(lane_id_); - if (!intersection_enter_point_opt) { - RCLCPP_DEBUG(logger_, "No intersection enter point found."); - return false; - } - - geometry_msgs::msg::Pose intersection_enter_pose; - 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.value(); + return std::nullopt; } - - stop_idx_ip = std::max(stop_idx_ip - base2front_idx_dist, 0); + stop_idx_default_ip = std::max(first_idx_conflicting_lane_opt.value() - 1 - margin_idx_dist, 0); + stop_idx_critical_ip = std::max(first_idx_conflicting_lane_opt.value() - 1, 0); } /* insert stop_point to use interpolated path*/ - *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + const size_t stopline_idx_default = insertPoint(stop_idx_default_ip, path_ip, path); + const size_t stopline_idx_critical = insertPoint(stop_idx_critical_ip, path_ip, path); - /* if another stop point exist before intersection stop_line, disable judge_line. */ - bool has_prior_stopline = false; - for (int i = 0; i < *stop_line_idx; ++i) { - if (std::fabs(path->points.at(i).point.longitudinal_velocity_mps) < 0.1) { - has_prior_stopline = true; - break; - } - } - - RCLCPP_DEBUG( - logger_, "generateStopLine() : stop_idx = %d, stop_idx_ip = %d, has_prior_stopline = %d", - *stop_line_idx, stop_idx_ip, has_prior_stopline); - - return true; + return std::make_pair(stopline_idx_default, stopline_idx_critical); } void BlindSpotModule::cutPredictPathWithDuration( @@ -385,36 +358,61 @@ 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.value().detection_areas; - debug_data_.conflict_areas_for_blind_spot = areas_opt.value().conflict_areas; + if (areas_opt) { + const auto & detection_areas = areas_opt.value().detection_areas; + const auto & conflict_areas = areas_opt.value().conflict_areas; + const auto & opposite_detection_areas = areas_opt.value().opposite_detection_areas; + const auto & opposite_conflict_areas = areas_opt.value().opposite_conflict_areas; + debug_data_.detection_areas = detection_areas; + debug_data_.conflict_areas = conflict_areas; + debug_data_.detection_areas.insert( + debug_data_.detection_areas.end(), opposite_detection_areas.begin(), + opposite_detection_areas.end()); + debug_data_.conflict_areas.insert( + debug_data_.conflict_areas.end(), opposite_conflict_areas.begin(), + opposite_conflict_areas.end()); autoware_auto_perception_msgs::msg::PredictedObjects objects = *objects_ptr; cutPredictPathWithDuration(&objects, planner_param_.max_future_movement_time); // check objects in blind spot areas - bool obstacle_detected = false; for (const auto & object : objects.objects) { if (!isTargetObjectType(object)) { continue; } - const auto & detection_areas = areas_opt.value().detection_areas; - const auto & conflict_areas = areas_opt.value().conflict_areas; - const bool exist_in_detection_area = + // right direction + const bool exist_in_right_detection_area = std::any_of(detection_areas.begin(), detection_areas.end(), [&object](const auto & area) { return bg::within( to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), lanelet::utils::to2D(area)); }); - const bool exist_in_conflict_area = + // opposite direction + const bool exist_in_opposite_detection_area = std::any_of( + opposite_detection_areas.begin(), opposite_detection_areas.end(), + [&object](const auto & area) { + return bg::within( + to_bg2d(object.kinematics.initial_pose_with_covariance.pose.position), + lanelet::utils::to2D(area)); + }); + const bool exist_in_detection_area = + exist_in_right_detection_area || exist_in_opposite_detection_area; + if (!exist_in_detection_area) { + continue; + } + const bool exist_in_right_conflict_area = isPredictedPathInArea(object, conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_opposite_conflict_area = isPredictedPathInArea( + object, opposite_conflict_areas, planner_data_->current_odometry->pose); + const bool exist_in_conflict_area = + exist_in_right_conflict_area || exist_in_opposite_conflict_area; if (exist_in_detection_area && exist_in_conflict_area) { - obstacle_detected = true; debug_data_.conflicting_targets.objects.push_back(object); + return true; } } - return obstacle_detected; + return false; } else { return false; } @@ -504,6 +502,37 @@ lanelet::ConstLanelet BlindSpotModule::generateExtendedAdjacentLanelet( return new_lanelet; } +lanelet::ConstLanelet BlindSpotModule::generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const +{ + const auto centerline = lanelet.centerline2d(); + const auto width = + boost::geometry::area(lanelet.polygon2d().basicPolygon()) / boost::geometry::length(centerline); + const double extend_width = + std::min(planner_param_.opposite_adjacent_extend_width, width); + const auto left_bound_ = + direction == TurnDirection::RIGHT + ? lanelet.rightBound().invert() + : lanelet::utils::getCenterlineWithOffset(lanelet.invert(), -width / 2 + extend_width); + const auto right_bound_ = + direction == TurnDirection::RIGHT + ? lanelet::utils::getCenterlineWithOffset(lanelet.invert(), width / 2 - extend_width) + : lanelet.leftBound().invert(); + lanelet::Points3d lefts, rights; + for (const auto & pt : left_bound_) { + lefts.push_back(lanelet::Point3d(pt)); + } + for (const auto & pt : right_bound_) { + rights.push_back(lanelet::Point3d(pt)); + } + const auto left_bound = lanelet::LineString3d(lanelet::InvalId, std::move(lefts)); + const auto right_bound = lanelet::LineString3d(lanelet::InvalId, std::move(rights)); + auto new_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); + const auto new_centerline = lanelet::utils::generateFineCenterline(new_lanelet, 5.0); + new_lanelet.setCenterline(new_centerline); + return new_lanelet; +} + std::optional BlindSpotModule::generateBlindSpotPolygons( lanelet::LaneletMapConstPtr lanelet_map_ptr, [[maybe_unused]] lanelet::routing::RoutingGraphPtr routing_graph_ptr, @@ -512,19 +541,20 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( { std::vector lane_ids; lanelet::ConstLanelets blind_spot_lanelets; + lanelet::ConstLanelets blind_spot_opposite_lanelets; /* get lane ids until intersection */ for (const auto & point : path.points) { bool found_intersection_lane = false; for (const auto lane_id : point.lane_ids) { - // make lane_ids unique - if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { - lane_ids.push_back(lane_id); - } - if (lane_id == lane_id_) { found_intersection_lane = true; + lane_ids.push_back(lane_id); break; } + // make lane_ids unique + if (std::find(lane_ids.begin(), lane_ids.end(), lane_id) == lane_ids.end()) { + lane_ids.push_back(lane_id); + } } if (found_intersection_lane) break; } @@ -537,27 +567,49 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( // additional detection area on left/right side lanelet::ConstLanelets adjacent_lanelets; + lanelet::ConstLanelets opposite_adjacent_lanelets; for (const auto i : lane_ids) { const auto lane = lanelet_map_ptr->laneletLayer.get(i); - const auto adj = std::invoke([&]() -> std::optional { - if (turn_direction_ == TurnDirection::INVALID) { + const auto adj = + turn_direction_ == TurnDirection::LEFT + ? (routing_graph_ptr->adjacentLeft(lane)) + : (turn_direction_ == TurnDirection::RIGHT ? (routing_graph_ptr->adjacentRight(lane)) + : boost::none); + const std::optional opposite_adj = + [&]() -> std::optional { + if (!!adj) { return std::nullopt; } - const auto adj_lane = (turn_direction_ == TurnDirection::LEFT) - ? routing_graph_ptr->adjacentLeft(lane) - : routing_graph_ptr->adjacentRight(lane); - - if (adj_lane) { - return *adj_lane; + if (turn_direction_ == TurnDirection::LEFT) { + // this should exist in right-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.leftBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); } - - return std::nullopt; - }); - + if (turn_direction_ == TurnDirection::RIGHT) { + // this should exist in left-hand traffic + const auto adjacent_lanes = + lanelet_map_ptr->laneletLayer.findUsages(lane.rightBound().invert()); + if (adjacent_lanes.empty()) { + return std::nullopt; + } + return adjacent_lanes.front(); + } else { + return std::nullopt; + } + }(); if (adj) { const auto half_lanelet = generateExtendedAdjacentLanelet(adj.value(), turn_direction_); adjacent_lanelets.push_back(half_lanelet); } + if (opposite_adj) { + const auto half_lanelet = + generateExtendedOppositeAdjacentLanelet(opposite_adj.value(), turn_direction_); + opposite_adjacent_lanelets.push_back(half_lanelet); + } } const auto current_arc_ego = @@ -587,6 +639,24 @@ std::optional BlindSpotModule::generateBlindSpotPolygons( blind_spot_polygons.conflict_areas.emplace_back(std::move(conflicting_area_adj)); blind_spot_polygons.detection_areas.emplace_back(std::move(detection_area_adj)); } + // additional detection area on left/right opposite lane side + if (!opposite_adjacent_lanelets.empty()) { + const auto stop_line_arc_opposite_adj = + lanelet::utils::getLaneletLength3d(opposite_adjacent_lanelets); + const auto current_arc_opposite_adj = + stop_line_arc_opposite_adj - (stop_line_arc_ego - current_arc_ego); + const auto detection_area_start_length_opposite_adj = + stop_line_arc_opposite_adj - planner_param_.backward_length; + auto conflicting_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, current_arc_opposite_adj, stop_line_arc_opposite_adj); + auto detection_area_opposite_adj = lanelet::utils::getPolygonFromArcLength( + opposite_adjacent_lanelets, detection_area_start_length_opposite_adj, + stop_line_arc_opposite_adj); + blind_spot_polygons.opposite_conflict_areas.emplace_back( + std::move(conflicting_area_opposite_adj)); + blind_spot_polygons.opposite_detection_areas.emplace_back( + std::move(detection_area_opposite_adj)); + } return blind_spot_polygons; } else { return std::nullopt; @@ -677,7 +747,7 @@ lanelet::ConstLanelets BlindSpotModule::getStraightLanelets( const auto next_lanelets = routing_graph_ptr->following(prev_intersection_lanelets.front()); for (const auto & ll : next_lanelets) { const std::string turn_direction = ll.attributeOr("turn_direction", "else"); - if (!turn_direction.compare("straight")) { + if (turn_direction.compare("straight") == 0) { straight_lanelets.push_back(ll); } } diff --git a/planning/behavior_velocity_blind_spot_module/src/scene.hpp b/planning/behavior_velocity_blind_spot_module/src/scene.hpp index 3915acd3532b5..6f8450568939c 100644 --- a/planning/behavior_velocity_blind_spot_module/src/scene.hpp +++ b/planning/behavior_velocity_blind_spot_module/src/scene.hpp @@ -30,6 +30,7 @@ #include #include +#include #include namespace behavior_velocity_planner @@ -38,6 +39,8 @@ struct BlindSpotPolygons { std::vector conflict_areas; std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; }; class BlindSpotModule : public SceneModuleInterface @@ -50,8 +53,10 @@ class BlindSpotModule : public SceneModuleInterface geometry_msgs::msg::Pose virtual_wall_pose; geometry_msgs::msg::Pose stop_point_pose; geometry_msgs::msg::Pose judge_point_pose; - std::vector conflict_areas_for_blind_spot; - std::vector detection_areas_for_blind_spot; + std::vector conflict_areas; + std::vector detection_areas; + std::vector opposite_conflict_areas; + std::vector opposite_detection_areas; autoware_auto_perception_msgs::msg::PredictedObjects conflicting_targets; }; @@ -67,6 +72,7 @@ class BlindSpotModule : public SceneModuleInterface double threshold_yaw_diff; //! threshold of yaw difference between ego and target object double adjacent_extend_width; //! the width of extended detection/conflict area on adjacent lane + double opposite_adjacent_extend_width; }; BlindSpotModule( @@ -118,6 +124,8 @@ class BlindSpotModule : public SceneModuleInterface lanelet::ConstLanelet generateExtendedAdjacentLanelet( const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; + lanelet::ConstLanelet generateExtendedOppositeAdjacentLanelet( + const lanelet::ConstLanelet lanelet, const TurnDirection direction) const; /** * @brief Make blind spot areas. Narrow area is made from closest path point to stop line index. @@ -169,9 +177,9 @@ class BlindSpotModule : public SceneModuleInterface * @param pass_judge_line_idx generated pass judge line index * @return false when generation failed */ - bool generateStopLine( + std::optional> generateStopLine( const lanelet::ConstLanelets straight_lanelets, - autoware_auto_planning_msgs::msg::PathWithLaneId * path, int * stop_line_idx) const; + autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; /** * @brief Insert a point to target path diff --git a/planning/behavior_velocity_crosswalk_module/README.md b/planning/behavior_velocity_crosswalk_module/README.md index 5f82fe839fda6..37c3fb58049a6 100644 --- a/planning/behavior_velocity_crosswalk_module/README.md +++ b/planning/behavior_velocity_crosswalk_module/README.md @@ -1,204 +1,169 @@ -## Crosswalk +# Crosswalk -### Role +## Role -This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of pedestrians and bicycles based on object's behavior and surround traffic. +This module judges whether the ego should stop in front of the crosswalk in order to provide safe passage of crosswalk users such as pedestrians and bicycles based on the objects' behavior and surround traffic.
- ![example](docs/example.png){width=1000} -
crosswalk module
+ ![crosswalk_module](docs/crosswalk_module.svg){width=1100}
-### Activation Timing +## Features -The manager launch crosswalk scene modules when the reference path conflicts crosswalk lanelets. +### Yield -### Module Parameters +#### Target Object -#### Common parameters +The target object's type is filtered by the following parameters in the `object_filtering.target_object` namespace. -| Parameter | Type | Description | -| ----------------------------- | ---- | ------------------------------- | -| `common.show_processing_time` | bool | whether to show processing time | +| Parameter | Unit | Type | Description | +| ------------ | ---- | ---- | ---------------------------------------------- | +| `unknown` | [-] | bool | whether to look and stop by UNKNOWN objects | +| `pedestrian` | [-] | bool | whether to look and stop by PEDESTRIAN objects | +| `bicycle` | [-] | bool | whether to look and stop by BICYCLE objects | +| `motorcycle` | [-] | bool | whether to look and stop by MOTORCYCLE objects | -#### Parameters for input data +In order to detect crosswalk users crossing outside the crosswalk as well, the crosswalk module creates an attention area around the crosswalk shown as the yellow polygon in the figure. If the object's predicted path collides with the attention area, the object will be targeted for yield. -| Parameter | Type | Description | -| ------------------------------------ | ------ | ---------------------------------------------- | -| `common.traffic_light_state_timeout` | double | [s] timeout threshold for traffic light signal | +
+ ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=600} +
-#### Parameters for stop position +The parameter is in the `object_filtering.target_object` namespace. -The crosswalk module determines a stop position at least `stop_distance_from_object` away from the object. +| Parameter | Unit | Type | Description | +| --------------------------- | ---- | ------ | ------------------------------------------------------------------------------------------------- | +| `crosswalk_attention_range` | [m] | double | the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -
- ![stop_distance_from_object](docs/stop_margin.svg){width=1000} -
stop margin
-
+#### Stop Position -The stop line is the reference point for the stopping position of the vehicle, but if there is no stop line in front of the crosswalk, the position `stop_distance_from_crosswalk` meters before the crosswalk is the virtual stop line for the vehicle. Then, if the stop position determined from `stop_distance_from_object` exists in front of the stop line determined from the HDMap or `stop_distance_from_crosswalk`, the actual stop position is determined according to `stop_distance_from_object` in principle, and vice versa. +First of all, `stop_distance_from_object [m]` is always kept at least between the ego and the target object for safety. -
- ![stop_line](docs/stop_line.svg){width=700} -
explicit stop line
-
+When the stop line exists in the lanelet map, the stop position is calculated based on the line. +When the stop line does **NOT** exist in the lanelet map, the stop position is calculated by keeping `stop_distance_from_crosswalk [m]` between the ego and the crosswalk. -
- ![stop_distance_from_crosswalk](docs/stop_line_distance.svg){width=700} -
virtual stop point
-
+
+ + + + + +
+
On the other hand, if pedestrian (bicycle) is crossing **wide** crosswalks seen in scramble intersections, and the pedestrian position is more than `far_object_threshold` meters away from the stop line, the actual stop position is determined to be `stop_distance_from_object` and pedestrian position, not at the stop line.
- ![far_object_threshold](docs/stop_line_margin.svg){width=1000} -
stop at wide crosswalk
+ ![far_object_threshold](docs/far_object_threshold.drawio.svg){width=700}
-See the workflow in algorithms section. +In the `stop_position` namespace, -| Parameter | Type | Description | -| -------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `stop_position.stop_distance_from_object` | double | [m] the vehicle decelerates to be able to stop in front of object with margin | -| `stop_position.stop_distance_from_crosswalk` | double | [m] make stop line away from crosswalk when no explicit stop line exists | -| `stop_position.far_object_threshold` | double | [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) | -| `stop_position.stop_position_threshold` | double | [m] threshold for check whether the vehicle stop in front of crosswalk | +| Parameter | | Type | Description | +| ------------------------------ | --- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `stop_position_threshold` | [m] | double | If the ego vehicle has stopped near the stop line than this value, this module assumes itself to have achieved yielding. | +| `stop_distance_from_crosswalk` | [m] | double | make stop line away from crosswalk for the Lanelet2 map with no explicit stop lines | +| `far_object_threshold` | [m] | double | If objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_distance_from_object meters before the object) for the case where the crosswalk width is very wide | +| `stop_distance_from_object` | [m] | double | the vehicle decelerates to be able to stop in front of object with margin | -#### Parameters for ego's slow down velocity +#### Yield decision -| Parameter | Type | Description | -| --------------------- | ------ | --------------------------------------------------------------------------------------------------------------------------- | -| `slow_velocity` | double | [m/s] target vehicle velocity when module receive slow down command from FOA | -| `max_slow_down_jerk` | double | [m/sss] minimum jerk deceleration for safe brake | -| `max_slow_down_accel` | double | [m/ss] minimum accel deceleration for safe brake | -| `no_relax_velocity` | double | [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | +The module makes a decision to yield only when the pedestrian traffic light is **GREEN** or **UNKNOWN**. +Calculating the collision point, the decision is based on the following variables. -#### Parameters for stuck vehicle +- TTC: Time-To-Collision which is the time for the **ego** to reach the virtual collision point. +- TTV: Time-To-Vehicle which is the time for the **object** to reach the virtual collision point. -If there are low speed or stop vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle (see following figure), closing the distance to that vehicle could cause Ego to be stuck on the crosswalk. So, in this situation, this module plans to stop before the crosswalk and wait until the vehicles move away, even if there are no pedestrians or bicycles. +Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories based on [1] -
- ![stuck_vehicle_attention_range](docs/stuck_vehicle_attention_range.svg){width=1000} -
stuck vehicle attention range
-
+- A. **TTC >> TTV**: The object has enough time to cross before the ego. + - No stop planning. +- B. **TTC ≒ TTV**: There is a risk of collision. + - **Stop point is inserted in the ego's path**. +- C. **TTC << TTV**: Ego has enough time to cross before the object. + - No stop planning. -| Parameter | Type | Description | -| ------------------------------------------------ | ------ | ---------------------------------------------------------------------- | -| `stuck_vehicle.stuck_vehicle_velocity` | double | [m/s] maximum velocity threshold whether the vehicle is stuck | -| `stuck_vehicle.max_stuck_vehicle_lateral_offset` | double | [m] maximum lateral offset for stuck vehicle position should be looked | -| `stuck_vehicle.stuck_vehicle_attention_range` | double | [m] the detection area is defined as X meters behind the crosswalk | +
+ + + + + +
+
-#### Parameters for pass judge logic +The boundary of A and B is interpolated from `ego_pass_later_margin_x` and `ego_pass_later_margin_y`. In the case of the upper figure, `ego_pass_later_margin_x` is `{0, 1, 2}` and `ego_pass_later_margin_y` is `{1, 4, 6}` for example. +The same way, the boundary of B and C is calculated from `ego_pass_first_margin_x` and `ego_pass_first_margin_y`. In the case of the upper figure, `ego_pass_first_margin_x` is `{3, 5}` and `ego_pass_first_margin_y` is `{0, 1}` for example. -Also see algorithm section. +In the `pass_judge` namespace, -| Parameter | Type | Description | -| ------------------------------------------- | ------ | -------------------------------------------------------------------------------------------------------------------------------------------------- | -| `pass_judge.ego_pass_first_margin` | double | [s] time margin for ego pass first situation | -| `pass_judge.ego_pass_later_margin` | double | [s] time margin for object pass first situation | -| `pass_judge.stop_object_velocity_threshold` | double | [m/s] velocity threshold for the module to judge whether the objects is stopped | -| `pass_judge.min_object_velocity` | double | [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV.) | -| `pass_judge.timeout_no_intention_to_walk` | double | [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. | -| `pass_judge.timeout_ego_stop_for_yield` | double | [s] the amount of time which ego should be stopping to query whether it yields or not. | +| Parameter | | Type | Description | +| ---------------------------------- | ----- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `ego_pass_first_margin_x` | [[s]] | double | time to collision margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_margin_y` | [[s]] | double | time to vehicle margin vector for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) | +| `ego_pass_first_additional_margin` | [s] | double | additional time margin for ego pass first situation to suppress chattering | +| `ego_pass_later_margin_x` | [[s]] | double | time to vehicle margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_margin_y` | [[s]] | double | time to collision margin vector for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) | +| `ego_pass_later_additional_margin` | [s] | double | additional time margin for object pass first situation to suppress chattering | -#### Parameters for object filtering +### Smooth Yield Decision -As a countermeasure against pedestrians attempting to cross outside the crosswalk area, this module watches not only the crosswalk zebra area but also in front of and behind space of the crosswalk, and if there are pedestrians or bicycles attempting to pass through the watch area, this module judges whether ego should pass or stop. +When the object is stopped around the crosswalk but has no intention to walk, the ego will yield the object forever. +To prevent this dead lock behavior, the ego will cancel the yield depending on the situation. -
- ![crosswalk_attention_range](docs/crosswalk_attention_range.svg){width=1000} -
crosswalk attention range
-
+#### When there is no traffic light -This module mainly looks the following objects as target objects. There are also optional flags that enables the pass/stop decision for `motorcycle` and `unknown` objects. - -- pedestrian -- bicycle - -| Parameter | Type | Description | -| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------- | -| `crosswalk_attention_range` | double | [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk | -| `target/unknown` | bool | whether to look and stop by UNKNOWN objects | -| `target/bicycle` | bool | whether to look and stop by BICYCLE objects | -| `target/motorcycle` | bool | whether to look and stop MOTORCYCLE objects | -| `target/pedestrian` | bool | whether to look and stop PEDESTRIAN objects | - -### Inner-workings / Algorithms - -#### Stop position - -The stop position is determined by the existence of the stop line defined by the HDMap, the positional relationship between the stop line and the pedestrians and bicycles, and each parameter. - -```plantuml -start -:calculate stop point from **stop_distance_from_object** (POINT-1); -if (There is the stop line in front of the crosswalk?) then (yes) - :calculate stop point from stop line (POINT-2.1); -else (no) - :calculate stop point from **stop_distance_from_crosswalk** (POINT-2.2); -endif -if (The distance ego to **POINT-1** is shorter than the distance ego to **POINT-2**) then (yes) - :ego stops at POINT-1; -else if (The distance ego to **POINT-1** is longer than the distance ego to **POINT-2** + **far_object_threshold**) then (yes) - :ego stops at POINT-1; -else (no) - :ego stops at POINT-2; -endif -end -``` +For the object stopped around the crosswalk but has no intention to walk (\*1), when the ego keeps stopping to yield for a certain time (\*2), the ego cancels the yield and start driving. -#### Pass judge logic +\*1: +The time is calculated by the interpolation of distance between the object and crosswalk with `distance_map_for_no_intention_to_walk` and `timeout_map_for_no_intention_to_walk`. -At first, this module determines whether the pedestrians or bicycles are likely to cross the crosswalk based on the color of the pedestrian traffic light signal related to the crosswalk. Only when the pedestrian traffic signal is **RED**, this module judges that the objects will not cross the crosswalk and skip the pass judge logic. +In the `pass_judge` namespace, -Secondly, this module makes a decision as to whether ego should stop in front of the crosswalk or pass through based on the relative relationship between TTC(Time-To-Collision) and TTV(Time-To-Vehicle). The TTC is the time it takes for ego to reach the virtual collision point, and the TTV is the time it takes for the object to reach the virtual collision point. +| Parameter | | Type | Description | +| --------------------------------------- | ----- | ------ | --------------------------------------------------------------------------------- | +| `distance_map_for_no_intention_to_walk` | [[m]] | double | distance map to calculate the timeout for no intention to walk with interpolation | +| `timeout_map_for_no_intention_to_walk` | [[s]] | double | timeout map to calculate the timeout for no intention to walk with interpolation | -
- ![virtual_collision_point](docs/virtual_collision_point.svg){width=1000} -
virtual collision point
-
+\*2: +In the `pass_judge` namespace, -Depending on the relative relationship between TTC and TTV, the ego's behavior at crosswalks can be classified into three categories. +| Parameter | | Type | Description | +| ---------------------------- | --- | ------ | ----------------------------------------------------------------------------------------------------------------------- | +| `timeout_ego_stop_for_yield` | [s] | double | If the ego maintains the stop for this amount of time, then the ego proceeds, assuming it has stopped long time enough. | -1. **TTC >> TTV**: The objects have enough time to cross first before ego reaches the crosswalk. (Type-A) -2. **TTC ≒ TTV**: There is a risk of a near miss and collision between ego and objects at the virtual collision point. (Type-B) -3. **TTC << TTV**: Ego has enough time to path through the crosswalk before the objects reach the virtual collision point. (Type-C) +#### When there is traffic light -This module judges that ego is able to pass through the crosswalk without collision risk when the relative relationship between TTC and TTV is **Type-A** and **Type-C**. On the other hand, this module judges that ego needs to stop in front of the crosswalk prevent collision with objects in **Type-B** condition. The time margin can be set by parameters `ego_pass_first_margin` and `ego_pass_later_margin`. This logic is designed based on [1]. +For the object stopped around the crosswalk but has no intention to walk (\*1), the ego will cancel the yield without stopping. +This comes from the assumption that the object has no intention to walk since it is stopped even though the pedestrian traffic light is green. -
- ![ttc-ttv](docs/ttc-ttv.svg){width=1000} -
time-to-collision vs time-to-vehicle
-
+\*1: +The crosswalk user's intention to walk is calculated in the same way as `When there is no traffic light`. -This module uses the larger value of estimated object velocity and `min_object_velocity` in calculating TTV in order to avoid division by zero. - -```plantuml -start -if (Pedestrian's traffic light signal is **RED**?) then (yes) -else (no) - if (There are objects around the crosswalk?) then (yes) - :calculate TTC & TTV; - if (TTC < TTV + **ego_pass_first_margin** && TTV < TTC + **ego_pass_later_margin**) then (yes) - :STOP; - else (no) - :PASS; - endif - endif -endif -end -``` +
+ + + + + +
+
-#### Dead lock prevention +#### New Object Handling -If there are objects stop within a radius of `min_object_velocity * ego_pass_later_margin` meters from virtual collision point, this module judges that ego should stop based on the pass judge logic described above at all times. In such a situation, even if the pedestrian has no intention of crossing, ego continues the stop decision on the spot. So, this module has another logic for dead lock prevention, and if the object continues to stop for more than `timeout_no_intention_to_walk` seconds after ego stops in front of the crosswalk, this module judges that the object has no intention of crossing and switches from **STOP** state to **PASS** state. The parameter `stop_object_velocity_threshold` is used to judge whether the objects are stopped or not. In addition, if the object starts to move after the module judges that the object has no intention of crossing, this module judges whether ego should stop or not once again. +Due to the perception's limited performance where the tree or poll is recognized as a pedestrian or the tracking failure in the crowd or occlusion, even if the surrounding environment does not change, the new pedestrian (= the new ID's pedestrian) may suddenly appear unexpectedly. +When this happens when the ego is going to pass the crosswalk, the ego will stop suddenly. -
- ![no-intension](docs/no-intension.svg){width=1000} -
dead lock situation
-
+To fix this issue, the option `disable_yield_for_new_stopped_object` is prepared. +If set to true, the new stopped object will be ignored during the yield decision around the crosswalk with a traffic light. + +In the `pass_judge` namespace, + +| Parameter | | Type | Description | +| -------------------------------------- | --- | ---- | ------------------------------------------------------------------------------------------------ | +| `disable_yield_for_new_stopped_object` | [-] | bool | If set to true, the new stopped object will be ignored around the crosswalk with a traffic light | -#### Safety Slow Down Behavior +### Safety Slow Down Behavior In current autoware implementation if there are no target objects around a crosswalk, ego vehicle will not slow down for the crosswalk. However, if ego vehicle to slow down to a certain speed in @@ -207,27 +172,103 @@ it is instructed in [lanelet2_format_extension.md](https://github.com/autowarefoundation/autoware_common/blob/main/tmp/lanelet2_extension/docs/lanelet2_format_extension.md) document. -### Limitations +| Parameter | | Type | Description | +| --------------------- | ------- | ------ | --------------------------------------------------------------------------------------------------------------------- | +| `slow_velocity` | [m/s] | double | target vehicle velocity when module receive slow down command from FOA | +| `max_slow_down_jerk` | [m/sss] | double | minimum jerk deceleration for safe brake | +| `max_slow_down_accel` | [m/ss] | double | minimum accel deceleration for safe brake | +| `no_relax_velocity` | [m/s] | double | if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints) | -When multiple crosswalks are nearby (such as intersection), this module may make a stop decision even at crosswalks where the object has no intention of crossing. +### Stuck Vehicle Detection + +The feature will make the ego not to stop on the crosswalk. +When there are low-speed or stopped vehicle ahead of the crosswalk, and there is not enough space between the crosswalk and the vehicle, the crosswalk module will plan to stop before the crosswalk even if there are no pedestrians or bicycles. + +`min_acc`, `min_jerk`, and `max_jerk` are met. If the ego cannot stop before the crosswalk with these parameters, the stop position will move forward. + +
+ ![stuck_vehicle_attention_range](docs/stuck_vehicle_detection.svg){width=600} +
+ +In the `stuck_vehicle` namespace, + +| Parameter | Unit | Type | Description | +| ---------------------------------- | ------- | ------ | ----------------------------------------------------------------------- | +| `stuck_vehicle_velocity` | [m/s] | double | maximum velocity threshold whether the target vehicle is stopped or not | +| `max_stuck_vehicle_lateral_offset` | [m] | double | maximum lateral offset of the target vehicle position | +| `stuck_vehicle_attention_range` | [m] | double | detection area length ahead of the crosswalk | +| `min_acc` | [m/ss] | double | minimum acceleration to stop | +| `min_jerk` | [m/sss] | double | minimum jerk to stop | +| `max_jerk` | [m/sss] | double | maximum jerk to stop | + +### Others + +In the `common` namespace, + +| Parameter | Unit | Type | Description | +| ----------------------------- | ---- | ------ | ----------------------------------------------------------------------------------------------------------------------------------------------- | +| `show_processing_time` | [-] | bool | whether to show processing time | +| `traffic_light_state_timeout` | [s] | double | timeout threshold for traffic light signal | +| `enable_rtc` | [-] | bool | if true, the scene modules should be approved by (request to cooperate)rtc function. if false, the module can be run without approval from rtc. | + +## Known Issues + +- The yield decision may be sometimes aggressive or conservative depending on the case. + - The main reason is that the crosswalk module does not know the ego's position in the future. The detailed ego's position will be determined after the whole planning. + - Currently the module assumes that the ego will move with a constant velocity. + +## Debugging + +### Visualization of debug markers + +`/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/crosswalk` shows the following markers.
- ![limitation](docs/limitation.svg){width=1000} -
design limits
+ ![limitation](docs/debug_markers.png){width=1000}
-### Known Issues +- Yellow polygons + - Ego footprints' polygon to calculate the collision check. +- Pink polygons + - Object footprints' polygon to calculate the collision check. +- The color of crosswalk + - Considering the traffic light's color, red means the target crosswalk and white means the ignored crosswalk. +- Texts + - It shows the module id, TTC, TTV, and the module state. + +### Visualization of Time-To-Collision -### Debugging +```sh +ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py +``` -By `ros2 run behavior_velocity_crosswalk_module time_to_collision_plotter.py`, you can visualize the following figure of the ego and pedestrian's time to collision. +enables you to visualize the following figure of the ego and pedestrian's time to collision. The label of each plot is `-`.
![limitation](docs/time_to_collision_plot.png){width=1000} -
Plot of time to collision
-### References/External links +## Trouble Shooting + +### Behavior + +- Q. The ego stopped around the crosswalk even though there were no crosswalk user objects. + - A. See [Stuck Vehicle Detection](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection). +- Q. The crosswalk virtual wall suddenly appeared resulting in the sudden stop. + - A. There may be a crosswalk user started moving when the ego was close to the crosswalk. +- Q. The crosswalk module decides to stop even when the pedestrian traffic light is red. + - A. The lanelet map may be incorrect. The pedestrian traffic light and the crosswalk have to be related. +- Q. In the planning simulation, the crosswalk module does the yield decision to stop on all the crosswalks. + - A. This is because the pedestrian traffic light is unknown by default. In this case, the crosswalk does the yield decision for safety. + +### Parameter Tuning + +- Q. The ego's yield behavior is too conservative. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) +- Q. The ego's yield behavior is too aggressive. + - A. Tune `ego_pass_later_margin` described in [Yield Decision](https://autowarefoundation.github.io/autoware.universe/pr-5583/planning/behavior_velocity_crosswalk_module/#stuck-vehicle-detection) + +## References/External links [1] 佐藤 みなみ, 早坂 祥一, 清水 政行, 村野 隆彦, 横断歩行者に対するドライバのリスク回避行動のモデル化, 自動車技術会論文集, 2013, 44 巻, 3 号, p. 931-936. diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg index 270a61264fe66..26e6118756bd8 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_attention_range.svg @@ -1,213 +1,132 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ target
- %3CmxGraphModel%3E%3... + target - - - - - - -
-
- crosswalk_attention_range - [m] +
+ target
- crosswalk_attention_range [m] + target - - - - -
+
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - +
+ NOT + target
- %3Cmx... + NOT target - - -
+
-
- Module don't respond prediction path without attention range. -
-
-
- - Module don't respond predict... - - - - - - -
-
-
- Vehicle +
+ crosswalk_attention_range
- Vehicle + crosswalk_attention... - - + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg new file mode 100644 index 0000000000000..18225f188fddf --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/crosswalk_module.svg @@ -0,0 +1,98 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png b/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png new file mode 100644 index 0000000000000..15645a340dca8 Binary files /dev/null and b/planning/behavior_velocity_crosswalk_module/docs/debug_markers.png differ diff --git a/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg new file mode 100644 index 0000000000000..13ba18bbb7588 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/explicit_stop_line.svg @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+
+ stop line defined in the lanele... +
+
+ +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg new file mode 100644 index 0000000000000..6e4c152cd79f0 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/far_object_threshold.drawio.svg @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop line defined in the lanelet map +
+
+
+
+ stop line defined in the lanele... +
+
+ + + + + + + + + + + +
+
+
+ stop_distance_from_object +
+
+
+
+ stop_distance_from_... +
+
+ + + + + + + + + + + +
+
+
far_object_threshold <
+
+
+
+ far_object_threshold < +
+
+
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg b/planning/behavior_velocity_crosswalk_module/docs/limitation.svg deleted file mode 100644 index 3e507c0669a2b..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/limitation.svg +++ /dev/null @@ -1,164 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - -
-
-
- stop objects -
-
-
-
- stop objects -
-
- - - - - - - - - -
-
-
- waiting for the traffic light to turn green. -
-
-
-
- waiting for the traffic l... -
-
- - - -
-
-
- The pedestrian has no intention to cross via this route. -
-
-
-
- The pedestrian has no intent... -
-
- - - - -
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg b/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg deleted file mode 100644 index 803ce223d591a..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/no-intension.svg +++ /dev/null @@ -1,193 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - - - - - - - -
-
-
- min_object_velocity * ego_pass_later_margin - [m] -
-
-
-
- min_object_velocity * ego_pass_later_margin [... -
-
- - - - -
-
-
- yield... -
-
-
-
- yield... -
-
- - - -
-
-
- stop objects -
-
-
-
- stop objects -
-
- - - - - -
-
-
- don't have intention -
- to cross -
-
-
-
- don't have intentio... -
-
-
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg deleted file mode 100644 index c2d486a7d3541..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line.svg +++ /dev/null @@ -1,149 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
-
- stop line... -
-
- - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
-
- stop_distance_from_object [m] -
-
- -
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg deleted file mode 100644 index 3842601ae246c..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_distance.svg +++ /dev/null @@ -1,170 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - - - - - - - - - -
-
-
- stop_distance_from_crosswalk - [m] -
-
-
-
- stop_distance_from_crosswalk [m] -
-
- - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
-
- stop_distance_from_object [m] -
-
- - - - -
-
-
- stop point -
- (planned in module) -
-
-
-
- stop point... -
-
-
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg deleted file mode 100644 index ea7fd9be3ff41..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_line_margin.svg +++ /dev/null @@ -1,204 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - -
-
-
- far_object_threshold - [m] -
-
-
-
- far_object_threshold [m] -
-
- - - - - - - - -
-
-
- stop line -
- (defined by HDMap) -
-
-
-
- stop line... -
-
- - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
-
- stop_distance_from_object [m] -
-
- - - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- -
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg b/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg deleted file mode 100644 index 924f642ad08bb..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stop_margin.svg +++ /dev/null @@ -1,144 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - - - - -
-
-
- stop_distance_from_object - [m] -
-
-
-
- stop_distance_from_object [m] -
-
- - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - -
-
-
- The module determines a stop position at least - stop_distance_from_object - away from the object's predicted path. -
-
-
-
- The module determines a stop positio... -
-
-
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg deleted file mode 100644 index 3ac1a94fcec78..0000000000000 --- a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_attention_range.svg +++ /dev/null @@ -1,174 +0,0 @@ - - - - - - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - - - - -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22%22%20style%3D%22endArrow%3Dclassic%3Bhtml%3D1%3Brounded%3D0%3BexitX%3D0.25%3BexitY%3D0%3BexitDx%3D0%3BexitDy%3D0%3BentryX%3D0.5%3BentryY%3D0.442%3BentryDx%3D0%3BentryDy%3D0%3BentryPerimeter%3D0%3B%22%20edge%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20width%3D%2250%22%20height%3D%2250%22%20relative%3D%221%22%20as%3D%22geometry%22%3E%3CmxPoint%20x%3D%22851%22%20y%3D%221780%22%20as%3D%22sourcePoint%22%2F%3E%3CmxPoint%20x%3D%22890.04%22%20y%3D%221780%22%20as%3D%22targetPoint%22%2F%3E%3C%2FmxGeometry%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20i... -
-
- - - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - - - -
-
-
- stuck_vehicle_attention_range - [m] -
-
-
-
- stuck_vehicle_attention_range [... -
-
- - - - -
-
-
- Vehicle -
-
-
-
- Vehicle -
-
- - - - - - - -
-
-
- max_stuck_vehicle_lateral_offset - [m] -
-
-
-
- max_stuck_vehicle_lateral_offset [m] -
-
- -
- - - - Text is not SVG - cannot display - - -
diff --git a/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg new file mode 100644 index 0000000000000..c517be5bb9967 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/stuck_vehicle_detection.svg @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stuck_vehicle_attention_range +
+
+
+
+ stuck_vehicle_atten... +
+
+ + + + + + + + + + + + +
+
+
+ max_stuck_vehicle_lateral_offset +
+
+
+
+ max_stuck_vehicle_l... +
+
+ + +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg new file mode 100644 index 0000000000000..6eb1b25cf5642 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/ttc_vs_ttv.drawio.svg @@ -0,0 +1,210 @@ + + + + + + + + + + + + +
+
+
+ (5, 1) +
+
+
+
+ (5, 1) +
+
+ + + + +
+
+
+ (1, 4) +
+
+
+
+ (1, 4) +
+
+ + + + +
+
+
+ (2, 6) +
+
+
+
+ (2, 6) +
+
+ + + + + + + + +
+
+
+ (0, 1) +
+
+
+
+ (0, 1) +
+
+ + + + +
+
+
+ (3, 0) +
+
+
+
+ (3, 0) +
+
+ + + + +
+
+
+ Time-To-Vehicle [s] +
+
+
+
+ Time-To-Vehicle [s] +
+
+ + + + + + + + + + + + + + + + + +
+
+
+ Time-To-Collision [s] +
+
+
+
+ Time-To-Collision [s] +
+
+ + + + +
+
+
+ B +
+
+
+
+ B +
+
+ + + + +
+
+
+ A +
+
+
+
+ A +
+
+ + + + +
+
+
+ C +
+
+
+
+ C +
+
+
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg index 8084dc2fbd8b0..c409493483845 100644 --- a/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_collision_point.svg @@ -1,134 +1,65 @@ + - + - - - - - - - - -
-
-
- Ego -
-
-
-
- Ego -
-
- - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + -
-
-
- - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - - - %3CmxGraphModel%3E%3Croot%3E%3CmxCell%20id%3D%220%22%2F%3E%3CmxCell%20id%3D%221%22%20parent%3D%220%22%2F%3E%3CmxCell%20id%3D%222%22%20value%3D%22attention_range%22%20style%3D%22text%3Bhtml%3D1%3BstrokeColor%3Dnone%3BfillColor%3Dnone%3Balign%3Dcenter%3BverticalAlign%3Dmiddle%3BwhiteSpace%3Dwrap%3Brounded%3D0%3BfontSize%3D20%3BfontStyle%3D2%22%20vertex%3D%221%22%20parent%3D%221%22%3E%3CmxGeometry%20x%3D%22531%22%20y%3D%221610%22%20width%3D%22210%22%20height%3D%2230%22%20as%3D%22geometry%22%2F%3E%3C%2FmxCell%3E%3C%2Froot%3E%3C%2FmxGraphModel%3E - -
-
-
-
- %3Cmx... -
-
- - - - - - - - - -
-
-
- virtual collision point -
-
-
-
- virtual collision poi... -
-
- - - -
-
-
- path -
-
-
-
- path -
-
- - - -
+
-
- object's prediction path +
+ virtual collision point
- object's prediction p... + virtual collision point - + diff --git a/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg new file mode 100644 index 0000000000000..7d4b4017c2f1a --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/virtual_stop_line.svg @@ -0,0 +1,73 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ stop_distance_from_crosswalk +
+
+
+
+ stop_distance_from_... +
+
+ + + + + + + + + + + + + +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg new file mode 100644 index 0000000000000..a09f430e990b3 --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/with_traffic_light.svg @@ -0,0 +1,97 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ NOT + yield +
+
+
+
+ NOT yield +
+
+
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg new file mode 100644 index 0000000000000..d4dea5fb78d2e --- /dev/null +++ b/planning/behavior_velocity_crosswalk_module/docs/without_traffic_light.svg @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ yield +
+
+
+
+ yield +
+
+ +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md index 58c2ce59edd48..afb2381e628a4 100644 --- a/planning/behavior_velocity_intersection_module/README.md +++ b/planning/behavior_velocity_intersection_module/README.md @@ -88,7 +88,7 @@ To precisely calculate stop positions, the path is interpolated at the certain i - 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. +- If a stopline is associated with the intersection lane on the map, that line is used as default_stopline for collision detection. Otherwise the point which is `common.default_stopline_margin` meters behind first_attention_stopline is defined as 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. @@ -113,8 +113,8 @@ There are several behaviors depending on the scene. | 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 | +| NonOccludedCollisionStop | Ego detects no occlusion but detects collision | Ego stops at default_stopline | +| FirstWaitBeforeOcclusion | Ego detected occlusion when entering the intersection | Ego stops at default_stopline 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 | @@ -219,7 +219,7 @@ ros2 run behavior_velocity_intersection_module ttc.py --lane_id ## Occlusion detection -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. +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 default_stopline for `occlusion.temporal_stop_time_before_peeking`, and then slowly creeps toward occlusion_peeking_stopline. If `occlusion.creep_during_peeking.enable` is true `occlusion.creep_during_peeking.creep_velocity` is inserted up to occlusion_peeking_stopline. Otherwise only stop line is inserted. 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. @@ -241,7 +241,7 @@ The remaining time is visualized on the intersection_occlusion virtual wall. ### Occlusion handling at intersection without traffic light -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. +At intersection without traffic light, if occlusion is detected, ego makes a brief stop at 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) @@ -263,7 +263,7 @@ TTC parameter varies depending on the traffic light color/shape as follows. ### 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. +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 default_stopline. ### skip on AMBER @@ -281,18 +281,49 @@ 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. +Generally it is not tolerable for vehicles that have lower traffic priority to stop in the middle of the unprotected area in intersections, and they need to stop at the stop line beforehand if there will be any risk of collision, which introduces two requirements: -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. +1. The vehicle must start braking before the boundary of the unprotected area at least by the braking distance if it is supposed to stop +2. The vehicle must recognize upcoming vehicles and check safety beforehand with enough braking distance margin if it is supposed to go + 1. And the SAFE decision must be absolutely certain and remain to be valid for the future horizon so that the safety condition will be always satisfied while ego is driving inside the unprotected area. +3. (TODO): Since it is almost impossible to make perfectly safe decision beforehand given the limited detection range/velocity tracking performance, intersection module should plan risk-evasive acceleration velocity profile AND/OR relax lateral acceleration limit while ego is driving inside the unprotected area, if the safety decision is "betrayed" later due to the following reasons: + 1. The situation _turned out to be dangerous_ later, mainly because velocity tracking was underestimated or the object accelerated beyond TTC margin + 2. The situation _turned dangerous_ later, mainly because the object is suddenly detected out of nowhere -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. +The position which is before the boundary of unprotected area by the braking distance which is obtained by -- 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. +$$ +\dfrac{v_{\mathrm{ego}}^{2}}{2a_{\mathrm{max}}} + v_{\mathrm{ego}} * t_{\mathrm{delay}} +$$ + +is called pass_judge_line, and safety decision must be made before ego passes this position because ego does not stop anymore. + +1st_pass_judge_line is before the first upcoming lane, and at intersections with multiple upcoming lanes, 2nd_pass_judge_line is defined as the position which is before the centerline of the first attention lane by the braking distance. 1st/2nd_pass_judge_line are illustrated in the following figure. + +![pass-judge-line](./docs/pass-judge-line.drawio.svg) + +Intersection module will command to GO if + +- ego is over default_stopline(or `common.enable_pass_judge_before_default_stopline` is true) AND +- ego is over 1st_pass judge line AND +- ego judged SAFE previously AND +- (ego is over 2nd_pass_judge_line OR ego is between 1st and 2nd pass_judge_line but most probable collision is expected to happen in the 1st attention lane) + +because it is expected to stop or continue stop decision if + +1. ego is before default_stopline && `common.enable_pass_judge_before_default_stopline` is false OR + 1. reason: default_stopline is defined on the map and should be respected +2. ego is before 1st_pass_judge_line OR + 1. reason: it has enough braking distance margin +3. ego judged UNSAFE previously + 1. reason: ego is now trying to stop and should continue stop decision if collision is detected in later calculation +4. (ego is between 1st and 2nd pass_judge_line and the most probable collision is expected to happen in the 2nd attention lane) + +For the 3rd condition, it is possible that ego stops with some overshoot to the unprotected area while it is trying to stop for collision detection, because ego should keep stop decision while UNSAFE decision is made even if it passed 1st_pass_judge_line during deceleration. + +For the 4th condition, at intersections with 2nd attention lane, even if ego is over the 1st pass_judge_line, still intersection module commands to stop if the most probable collision is expected to happen in the 2nd attention lane. + +Also if `occlusion.enable` is true, the position of 1st_pass_judge line changes to occlusion_peeking_stopline if ego passed the original 1st_pass_judge_line position while ego is peeking. Otherwise ego could inadvertently judge that it passed 1st_pass_judge during peeking and then abort peeking. ## Data Structure @@ -375,17 +406,18 @@ entity TargetObject { ### 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 | +| 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 | +| `.enable_pass_judge_before_default_stopline` | bool | [-] flag not to stop before default_stopline even if ego is over pass_judge_line | ### stuck_vehicle/yield_stuck @@ -430,7 +462,7 @@ entity TargetObject { | `.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_time_before_peeking` | double | [s] temporal stop duration at default_stopline 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 | diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 4e9eb50f2a462..108e021240851 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,7 @@ max_accel: -2.8 max_jerk: -5.0 delay_response_time: 0.5 + enable_pass_judge_before_default_stopline: false stuck_vehicle: turn_direction: @@ -36,7 +37,6 @@ consider_wrong_direction_vehicle: false collision_detection_hold_time: 0.5 min_predicted_path_confidence: 0.05 - keep_detection_velocity_threshold: 0.833 velocity_profile: use_upstream: true minimum_upstream_velocity: 0.01 diff --git a/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg new file mode 100644 index 0000000000000..d1f488af7c2ac --- /dev/null +++ b/planning/behavior_velocity_intersection_module/docs/pass-judge-line.drawio.svg @@ -0,0 +1,473 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+
+ 1st_pass_judge_line +
+
+ + + + + + + + +
+
+
+ + 2nd_pass_judge_line_magin + +
+
+
+
+ 2nd_pass_judge_line_magin +
+
+ + + + + + + + +
+
+
+ + 2nd_pass_judge_line + +
+
+
+
+ 2nd_pass_judge_line +
+
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ + 1st_pass_judge_line + +
+
+
+
+ 1st_pass_judge_line +
+
+ + + + + + + + +
+ + + + Text is not SVG - cannot display + + +
diff --git a/planning/behavior_velocity_intersection_module/src/debug.cpp b/planning/behavior_velocity_intersection_module/src/debug.cpp index 5f103e0ecd3b0..350f6d774f7cf 100644 --- a/planning/behavior_velocity_intersection_module/src/debug.cpp +++ b/planning/behavior_velocity_intersection_module/src/debug.cpp @@ -90,7 +90,7 @@ visualization_msgs::msg::MarkerArray createPoseMarkerArray( marker_line.type = visualization_msgs::msg::Marker::LINE_STRIP; marker_line.action = visualization_msgs::msg::Marker::ADD; marker_line.pose.orientation = createMarkerOrientation(0, 0, 0, 1.0); - marker_line.scale = createMarkerScale(0.1, 0.0, 0.0); + marker_line.scale = createMarkerScale(0.2, 0.0, 0.0); marker_line.color = createMarkerColor(r, g, b, 0.999); const double yaw = tf2::getYaw(pose.orientation); @@ -283,11 +283,23 @@ visualization_msgs::msg::MarkerArray IntersectionModule::createDebugMarkerArray( &debug_marker_array, now); */ - if (debug_data_.pass_judge_wall_pose) { + if (debug_data_.first_pass_judge_wall_pose) { + const double r = debug_data_.passed_first_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_first_pass_judge ? 0.0 : 1.0; appendMarkerArray( createPoseMarkerArray( - debug_data_.pass_judge_wall_pose.value(), "pass_judge_wall_pose", module_id_, 0.7, 0.85, - 0.9), + debug_data_.first_pass_judge_wall_pose.value(), "first_pass_judge_wall_pose", module_id_, r, + g, 0.0), + &debug_marker_array, now); + } + + if (debug_data_.second_pass_judge_wall_pose) { + const double r = debug_data_.passed_second_pass_judge ? 1.0 : 0.0; + const double g = debug_data_.passed_second_pass_judge ? 0.0 : 1.0; + appendMarkerArray( + createPoseMarkerArray( + debug_data_.second_pass_judge_wall_pose.value(), "second_pass_judge_wall_pose", module_id_, + r, g, 0.0), &debug_marker_array, now); } diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index c92f16dd7b474..0f9faaaa901c1 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -61,6 +61,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); ip.common.delay_response_time = getOrDeclareParameter(node, ns + ".common.delay_response_time"); + ip.common.enable_pass_judge_before_default_stopline = + getOrDeclareParameter(node, ns + ".common.enable_pass_judge_before_default_stopline"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); @@ -92,8 +94,6 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) 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.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( diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index dad0c194b5c9f..358dca2414bb0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -1041,7 +1041,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto default_stopline_idx_opt = intersection_stoplines.default_stopline; const auto first_attention_stopline_idx_opt = intersection_stoplines.first_attention_stopline; const auto occlusion_peeking_stopline_idx_opt = intersection_stoplines.occlusion_peeking_stopline; - const auto default_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto first_pass_judge_line_idx = intersection_stoplines.first_pass_judge_line; + const auto second_pass_judge_line_idx = intersection_stoplines.second_pass_judge_line; const auto occlusion_wo_tl_pass_judge_line_idx = intersection_stoplines.occlusion_wo_tl_pass_judge_line; @@ -1221,13 +1222,22 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( prev_occlusion_status_ = occlusion_status; } - // TODO(Mamoru Sobue): this part needs more formal handling - const size_t pass_judge_line_idx = [=]() { + const size_t pass_judge_line_idx = [&]() { if (enable_occlusion_detection_) { - // if occlusion detection is enabled, pass_judge position is beyond the boundary of first - // attention area if (has_traffic_light_) { - return occlusion_stopline_idx; + // if ego passed the first_pass_judge_line while it is peeking to occlusion, then its + // position is occlusion_stopline_idx. Otherwise it is first_pass_judge_line + if (passed_1st_judge_line_while_peeking_) { + return occlusion_stopline_idx; + } + const bool is_over_first_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, first_pass_judge_line_idx); + if (is_occlusion_state && is_over_first_pass_judge_line) { + passed_1st_judge_line_while_peeking_ = true; + return occlusion_stopline_idx; + } else { + return first_pass_judge_line_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 @@ -1235,30 +1245,53 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( } else { // if there is no traffic light and occlusion is not detected, pass_judge position is // default - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; } } - return default_pass_judge_line_idx; + return first_pass_judge_line_idx; }(); - debug_data_.pass_judge_wall_pose = - planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); - const double vel_norm = std::hypot( - planner_data_->current_velocity->twist.linear.x, - planner_data_->current_velocity->twist.linear.y); - const bool keep_detection = - (vel_norm < planner_param_.collision_detection.keep_detection_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_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_stopline && is_over_pass_judge_line && is_go_out_) || + + const bool is_over_1st_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, pass_judge_line_idx); + if (is_over_1st_pass_judge_line && was_safe && !safely_passed_1st_judge_line_time_) { + safely_passed_1st_judge_line_time_ = clock_->now(); + } + debug_data_.first_pass_judge_wall_pose = + planning_utils::getAheadPose(pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_first_pass_judge = safely_passed_1st_judge_line_time_.has_value(); + const bool is_over_2nd_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose, second_pass_judge_line_idx); + if (is_over_2nd_pass_judge_line && was_safe && !safely_passed_2nd_judge_line_time_) { + safely_passed_2nd_judge_line_time_ = clock_->now(); + } + debug_data_.second_pass_judge_wall_pose = + planning_utils::getAheadPose(second_pass_judge_line_idx, baselink2front, *path); + debug_data_.passed_second_pass_judge = safely_passed_2nd_judge_line_time_.has_value(); + + if ( + ((is_over_default_stopline || + planner_param_.common.enable_pass_judge_before_default_stopline) && + is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval + /* + * This body is active if ego is + * - over the default stopline AND + * - over the 1st && 2nd pass judge line AND + * - previously safe + * , + * which means ego can stop even if it is over the 1st pass judge line but + * - before default stopline OR + * - before the 2nd pass judge line OR + * - or previously unsafe + * . + * In order for ego to continue peeking or collision detection when occlusion is detected after + * ego passed the 1st pass judge line, it needs to be + * - before the default stopline OR + * - before the 2nd pass judge line OR + * - previously unsafe + */ is_permanent_go_ = true; return IntersectionModule::Indecisive{"over the pass judge line. no plan needed"}; } @@ -1309,10 +1342,14 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( : (planner_param_.collision_detection.collision_detection_hold_time - collision_state_machine_.getDuration()); + // TODO(Mamoru Sobue): if ego is over 1st/2nd pass judge line and collision is expected at 1st/2nd + // pass judge line respectively, ego should go + const auto second_attention_stopline_idx = intersection_stoplines.second_attention_stopline; + const auto last_intersection_stopline_candidate_idx = + second_attention_stopline_idx ? second_attention_stopline_idx.value() : occlusion_stopline_idx; const bool has_collision = checkCollision( - *path, &target_objects, path_lanelets, closest_idx, - std::min(occlusion_stopline_idx, path->points.size() - 1), time_to_restart, - traffic_prioritized_level); + *path, &target_objects, path_lanelets, closest_idx, last_intersection_stopline_candidate_idx, + time_to_restart, traffic_prioritized_level); collision_state_machine_.setStateWithMarginTime( has_collision ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("collision state_machine"), *clock_); @@ -1808,16 +1845,19 @@ std::optional IntersectionModule::generateIntersectionSto second_attention_area, interpolated_path_info, local_footprint, baselink2front); if (first_footprint_inside_2nd_attention_ip_opt) { second_attention_stopline_ip_int = first_footprint_inside_2nd_attention_ip_opt.value(); + second_attention_stopline_valid = true; } } const auto second_attention_stopline_ip = second_attention_stopline_ip_int >= 0 ? static_cast(second_attention_stopline_ip_int) : 0; - // (8) second pass judge lie position on interpolated path - int second_pass_judge_ip_int = second_attention_stopline_ip_int - std::ceil(braking_dist / ds); + // (8) second pass judge line position on interpolated path. It is the same as first pass judge + // line if second_attention_lane is null + int second_pass_judge_ip_int = occlusion_wo_tl_pass_judge_line_ip; const auto second_pass_judge_line_ip = - static_cast(std::max(second_pass_judge_ip_int, 0)); + second_attention_area_opt ? static_cast(std::max(second_pass_judge_ip_int, 0)) + : first_pass_judge_line_ip; struct IntersectionStopLinesTemp { @@ -2803,8 +2843,6 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( return std::nullopt; } }(); - 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); @@ -2818,7 +2856,7 @@ TimeDistanceArray IntersectionModule::calcIntersectionPassingTime( (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0; const double passing_velocity = [=]() { if (use_upstream_velocity) { - if (has_upstream_stopline && i > upstream_stopline_ind) { + if (upstream_stopline_idx_opt && i > upstream_stopline_idx_opt.value()) { return minimum_upstream_velocity; } return std::max(average_velocity, minimum_ego_velocity); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 38feada725872..1a9cf74624fc0 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -48,7 +48,10 @@ struct DebugData std::optional collision_stop_wall_pose{std::nullopt}; std::optional occlusion_stop_wall_pose{std::nullopt}; std::optional occlusion_first_stop_wall_pose{std::nullopt}; - std::optional pass_judge_wall_pose{std::nullopt}; + std::optional first_pass_judge_wall_pose{std::nullopt}; + bool passed_first_pass_judge{false}; + bool passed_second_pass_judge{false}; + std::optional second_pass_judge_wall_pose{std::nullopt}; std::optional> attention_area{std::nullopt}; std::optional> occlusion_attention_area{std::nullopt}; std::optional ego_lane{std::nullopt}; @@ -256,8 +259,8 @@ struct IntersectionStopLines size_t first_pass_judge_line{0}; /** - * second_pass_judge_line is before second_attention_stopline by the braking distance. if its - * value is calculated negative, it is 0 + * second_pass_judge_line is before second_attention_stopline by the braking distance. if + * second_attention_lane is null, it is same as first_pass_judge_line */ size_t second_pass_judge_line{0}; @@ -344,6 +347,7 @@ class IntersectionModule : public SceneModuleInterface double max_accel; double max_jerk; double delay_response_time; + bool enable_pass_judge_before_default_stopline; } common; struct TurnDirection @@ -373,7 +377,6 @@ class IntersectionModule : public SceneModuleInterface bool consider_wrong_direction_vehicle; double collision_detection_hold_time; double min_predicted_path_confidence; - double keep_detection_velocity_threshold; struct VelocityProfile { bool use_upstream; @@ -606,7 +609,7 @@ class IntersectionModule : public SceneModuleInterface private: rclcpp::Node & node_; - const int64_t lane_id_; + const lanelet::Id lane_id_; const std::set associative_ids_; const std::string turn_direction_; const bool has_traffic_light_; @@ -621,6 +624,9 @@ class IntersectionModule : public SceneModuleInterface bool is_permanent_go_{false}; DecisionResult prev_decision_result_{Indecisive{""}}; OcclusionType prev_occlusion_status_; + bool passed_1st_judge_line_while_peeking_{false}; + std::optional safely_passed_1st_judge_line_time_{std::nullopt}; + std::optional safely_passed_2nd_judge_line_time_{std::nullopt}; // for occlusion detection const bool enable_occlusion_detection_; diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index e2c29ef868257..9fa5634c6dd65 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -323,6 +323,11 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( { std::lock_guard lock(mutex_); + // clear previous observation + planner_data_.traffic_light_id_map_raw_.clear(); + const auto traffic_light_id_map_last_observed_old = + planner_data_.traffic_light_id_map_last_observed_; + planner_data_.traffic_light_id_map_last_observed_.clear(); for (const auto & signal : msg->signals) { TrafficSignalStamped traffic_signal; traffic_signal.stamp = msg->stamp; @@ -334,9 +339,12 @@ void BehaviorVelocityPlannerNode::onTrafficSignals( }); // if the observation is UNKNOWN and past observation is available, only update the timestamp // and keep the body of the info - if ( - is_unknown_observation && - planner_data_.traffic_light_id_map_last_observed_.count(signal.traffic_signal_id) == 1) { + const auto old_data = traffic_light_id_map_last_observed_old.find(signal.traffic_signal_id); + if (is_unknown_observation && old_data != traffic_light_id_map_last_observed_old.end()) { + // copy last observation + planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id] = + old_data->second; + // update timestamp planner_data_.traffic_light_id_map_last_observed_[signal.traffic_signal_id].stamp = msg->stamp; } else { diff --git a/vehicle/vehicle_info_util/Readme.md b/vehicle/vehicle_info_util/Readme.md index c3d06a3b43260..600fd62270d81 100644 --- a/vehicle/vehicle_info_util/Readme.md +++ b/vehicle/vehicle_info_util/Readme.md @@ -7,6 +7,35 @@ This package is to get vehicle info parameters. ### Description In [here](https://autowarefoundation.github.io/autoware-documentation/main/design/autoware-interfaces/components/vehicle-dimensions/), you can check the vehicle dimensions with more detail. +The current format supports only the Ackermann model. This file defines the model assumed in autoware path planning, control, etc. and does not represent the exact physical model. If a model other than the Ackermann model is used, it is assumed that a vehicle interface will be designed to change the control output for the model. + +### Versioning Policy + +We have implemented a versioning system for the `vehicle_info.param.yaml` file to ensure clarity and consistency in file format across different versions of Autoware and its external applications. Please see [discussion](https://github.com/orgs/autowarefoundation/discussions/4050) for the details. + +#### How to Operate + +- The current file format is set as an unversioned base version (`version:` field is commented out). +- For the next update involving changes (such as additions, deletions, or modifications): + - Uncomment and update the version line at the beginning of the file. + - Initiate versioning by assigning a version number, starting from `0.1.0`. Follow the semantic versioning format (MAJOR.MINOR.PATCH). + - Update this Readme.md too. +- For subsequent updates, continue incrementing the version number in accordance with the changes made. + - Discuss how to increment version depending on the amount of changes made to the file. + +```yaml +/**: + ros__parameters: + # version: 0.1.0 # Uncomment and update this line for future format changes. + wheel_radius: 0.383 + ... +``` + +#### Why Versioning? + +- Consistency Across Updates: Implementing version control will allow accurate tracking of changes over time and changes in vehicle information parameters. +- Clarity for External Applications: External applications that depend on `vehicle_info.param.yaml` need to reference the correct file version for optimal compatibility and functionality. +- Simplified Management for Customized Branches: Assigning versions directly to the `vehicle_info.param.yaml` file simplifies management compared to maintaining separate versions for multiple customized Autoware branches. This approach streamlines version tracking and reduces complexity. ### Scripts diff --git a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml index 8941b92b4e78e..72c070c17b52c 100644 --- a/vehicle/vehicle_info_util/config/vehicle_info.param.yaml +++ b/vehicle/vehicle_info_util/config/vehicle_info.param.yaml @@ -1,5 +1,6 @@ /**: ros__parameters: + # version: 0.1.0 # uncomment this line in the next update of this file format. please check Readme.md wheel_radius: 0.39 wheel_width: 0.42 wheel_base: 2.74 # between front wheel center and rear wheel center