From 58fdf86afb824d652fa4e8be2d02e98ac74d5dc4 Mon Sep 17 00:00:00 2001 From: Mehmet Dogru <42mehmetdogru42@gmail.com> Date: Mon, 24 Mar 2025 09:40:07 +0200 Subject: [PATCH] feat(crosswalk_module): consider objects on crosswalk when pedestrian traffic light is red Signed-off-by: Mehmet Dogru <42mehmetdogru42@gmail.com> --- .../config/crosswalk.param.yaml | 2 + .../src/manager.cpp | 2 + .../src/scene_crosswalk.cpp | 37 +++++++++++++------ .../src/scene_crosswalk.hpp | 1 + 4 files changed, 30 insertions(+), 12 deletions(-) diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml index 97e00ade8256c..e890ce611daef 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/config/crosswalk.param.yaml @@ -46,6 +46,8 @@ ego_pass_later_additional_margin: 0.5 # [s] additional time margin for object pass first situation to suppress chattering ego_min_assumed_speed: 2.0 # [m/s] assumed speed to calculate the time to collision point + consider_obj_on_crosswalk_on_red_light: true # consider and do not ignore objects if they are on the crosswalk when the crosswalk pedestrian traffic light is red + no_stop_decision: # parameters to determine stop cancel. {-$overrun_threshold_length + f($min_acc, $min_jerk)} is compared against distance to stop pose. min_acc: -1.5 # min acceleration [m/ss] min_jerk: -1.5 # min jerk [m/sss] diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp index 410fb51100924..a07e0e542dc49 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/manager.cpp @@ -106,6 +106,8 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) get_or_declare_parameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); cp.ego_min_assumed_speed = get_or_declare_parameter(node, ns + ".pass_judge.ego_min_assumed_speed"); + cp.consider_obj_on_crosswalk_on_red_light = + get_or_declare_parameter(node, ns + ".pass_judge.consider_obj_on_crosswalk_on_red_light"); cp.min_acc_for_no_stop_decision = get_or_declare_parameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); cp.min_jerk_for_no_stop_decision = diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 7ae6e16bb21e1..65cacb5a67a2b 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -1112,6 +1112,7 @@ void CrosswalkModule::updateObjectState( const double dist_ego_to_stop, const PathWithLaneId & sparse_resample_path, const std::pair & crosswalk_attention_range, const Polygon2d & attention_area) { + const auto & p = planner_param_; const auto & objects_ptr = planner_data_->predicted_objects; const auto traffic_lights_reg_elems = @@ -1120,37 +1121,48 @@ void CrosswalkModule::updateObjectState( // Check if ego is yielding const bool is_ego_yielding = [&]() { - const auto has_reached_stop_point = dist_ego_to_stop < planner_param_.stop_position_threshold; + const auto has_reached_stop_point = dist_ego_to_stop < p.stop_position_threshold; - return planner_data_->isVehicleStopped(planner_param_.timeout_ego_stop_for_yield) && - has_reached_stop_point; + return planner_data_->isVehicleStopped(p.timeout_ego_stop_for_yield) && has_reached_stop_point; }(); - const auto ignore_crosswalk = isRedSignalForPedestrians(); - debug_data_.ignore_crosswalk = ignore_crosswalk; + const auto is_red_signal_for_pedestrians = isRedSignalForPedestrians(); + auto ignore_crosswalk = is_red_signal_for_pedestrians; // Update object state object_info_manager_.init(); for (const auto & object : objects_ptr->objects) { - const auto obj_uuid = to_hex_string(object.object_id); - const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; - const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; - - // calculate collision point and state if (!isCrosswalkUserType(object)) { continue; } - if (ignore_crosswalk) { + + auto ignore_obj = is_red_signal_for_pedestrians; + if (p.consider_obj_on_crosswalk_on_red_light && is_red_signal_for_pedestrians) { + const auto is_object_on_crosswalk = + bg::intersects(autoware_utils::to_polygon2d(object), crosswalk_.polygon2d().basicPolygon()); + + if (is_object_on_crosswalk) { + ignore_obj = false; + ignore_crosswalk = false; + } + } + + if (ignore_obj) { continue; } + const auto obj_uuid = to_hex_string(object.object_id); + const auto & obj_pos = object.kinematics.initial_pose_with_covariance.pose.position; + const auto & obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear; + + // calculate collision point and state const auto collision_point = getCollisionPoint(sparse_resample_path, object, crosswalk_attention_range, attention_area); const std::optional ego_crosswalk_passage_direction = findEgoPassageDirectionAlongPath(sparse_resample_path); object_info_manager_.update( obj_uuid, obj_pos, std::hypot(obj_vel.x, obj_vel.y), clock_->now(), is_ego_yielding, - has_traffic_light, collision_point, object.classification.front().label, planner_param_, + has_traffic_light, collision_point, object.classification.front().label, p, crosswalk_.polygon2d().basicPolygon(), attention_area, ego_crosswalk_passage_direction); const auto collision_state = object_info_manager_.getCollisionState(obj_uuid); @@ -1178,6 +1190,7 @@ void CrosswalkModule::updateObjectState( getLabelColor(collision_state)); } + debug_data_.ignore_crosswalk = ignore_crosswalk; object_info_manager_.finalize(); } diff --git a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp index 7b85b07ee114f..88cd2aa998b4a 100644 --- a/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp +++ b/planning/behavior_velocity_planner/autoware_behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp @@ -149,6 +149,7 @@ class CrosswalkModule : public SceneModuleInterfaceWithRTC std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double ego_min_assumed_speed; + bool consider_obj_on_crosswalk_on_red_light; double min_acc_for_no_stop_decision; double min_jerk_for_no_stop_decision; double overrun_threshold_length_for_no_stop_decision;