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 0bd708f7b1705..949a8de361409 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 @@ -47,12 +47,18 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) // param for stop position cp.stop_distance_from_crosswalk = getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_crosswalk"); - cp.stop_distance_from_object = - getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object"); + cp.stop_distance_from_object_preferred = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_preferred"); + cp.stop_distance_from_object_limit = + getOrDeclareParameter(node, ns + ".stop_position.stop_distance_from_object_limit"); cp.far_object_threshold = getOrDeclareParameter(node, ns + ".stop_position.far_object_threshold"); cp.stop_position_threshold = getOrDeclareParameter(node, ns + ".stop_position.stop_position_threshold"); + cp.min_acc_preferred = + getOrDeclareParameter(node, ns + ".stop_position.min_acc_preferred"); + cp.min_jerk_preferred = + getOrDeclareParameter(node, ns + ".stop_position.min_jerk_preferred"); // param for restart suppression cp.min_dist_to_stop_for_restart_suppression = @@ -98,14 +104,12 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".pass_judge.ego_pass_later_additional_margin"); cp.ego_min_assumed_speed = getOrDeclareParameter(node, ns + ".pass_judge.ego_min_assumed_speed"); - cp.max_offset_to_crosswalk_for_yield = getOrDeclareParameter( - node, ns + ".pass_judge.no_stop_decision.max_offset_to_crosswalk_for_yield"); cp.min_acc_for_no_stop_decision = getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_acc"); - cp.max_jerk_for_no_stop_decision = - getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.max_jerk"); cp.min_jerk_for_no_stop_decision = getOrDeclareParameter(node, ns + ".pass_judge.no_stop_decision.min_jerk"); + cp.overrun_threshold_length_for_no_stop_decision = getOrDeclareParameter( + node, ns + ".pass_judge.no_stop_decision.overrun_threshold_length"); cp.stop_object_velocity = getOrDeclareParameter(node, ns + ".pass_judge.stop_object_velocity_threshold"); cp.min_object_velocity = 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 639ee214bca9c..c56ab1bd9fb70 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 @@ -316,15 +316,9 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk, const geometry_msgs::msg::Point & last_path_point_on_crosswalk, - const std::optional & default_stop_pose) + const std::optional & default_stop_pose_opt) { const auto & ego_pos = planner_data_->current_odometry->pose.position; - const double ego_vel = planner_data_->current_velocity->twist.linear.x; - const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - - const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; - const auto dist_ego_to_stop = - calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose->position); // Calculate attention range for crosswalk const auto crosswalk_attention_range = getAttentionRange( @@ -334,39 +328,36 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( const auto attention_area = getAttentionArea(sparse_resample_path, crosswalk_attention_range); // Update object state + // This exceptional handling should be done in update(), but is compromised by history + const double dist_default_stop = + default_stop_pose_opt.has_value() + ? calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position) + : std::numeric_limits::max(); updateObjectState( - dist_ego_to_stop, sparse_resample_path, crosswalk_attention_range, attention_area); - - // Check if ego moves forward enough to ignore yield. - const auto & p = planner_param_; - const double dist_ego2crosswalk = - calcSignedArcLength(ego_path.points, ego_pos, first_path_point_on_crosswalk); - const auto braking_distance_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( - ego_vel, 0.0, ego_acc, p.min_acc_for_no_stop_decision, p.max_jerk_for_no_stop_decision, - p.min_jerk_for_no_stop_decision); - const double braking_distance = braking_distance_opt ? *braking_distance_opt : 0.0; - if ( - dist_ego2crosswalk - base_link2front - braking_distance < p.max_offset_to_crosswalk_for_yield) { - return {}; - } + dist_default_stop, sparse_resample_path, crosswalk_attention_range, attention_area); // Check pedestrian for stop // NOTE: first stop point and its minimum distance from ego to stop auto isVehicleType = [](const uint8_t label) { return label == ObjectClassification::MOTORCYCLE || label == ObjectClassification::BICYCLE; }; - std::optional> nearest_stop_info; + std::optional dist_nearest_cp; std::vector stop_factor_points; const std::optional ego_crosswalk_passage_direction = findEgoPassageDirectionAlongPath(sparse_resample_path); for (const auto & object : object_info_manager_.getObject()) { const auto & collision_point_opt = object.collision_point; - if (collision_point_opt) { - const auto & collision_point = collision_point_opt.value(); + if (!object.collision_point || object.collision_state != CollisionState::YIELD) { + if (collision_point_opt) { + continue; + const auto & collision_point = collision_point_opt.value(); + } const auto & collision_state = object.collision_state; + const auto & collision_point = object.collision_point.value(); if (collision_state != CollisionState::YIELD) { continue; } + if ( isVehicleType(object.classification) && ego_crosswalk_passage_direction && collision_point.crosswalk_passage_direction) { @@ -381,39 +372,125 @@ std::optional CrosswalkModule::checkStopForCrosswalkUsers( stop_factor_points.push_back(object.position); const auto dist_ego2cp = - calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point) - - planner_param_.stop_distance_from_object; - if (!nearest_stop_info || dist_ego2cp - base_link2front < nearest_stop_info->second) { - nearest_stop_info = - std::make_pair(collision_point.collision_point, dist_ego2cp - base_link2front); + calcSignedArcLength(sparse_resample_path.points, ego_pos, collision_point.collision_point); + if (!dist_nearest_cp || dist_ego2cp < dist_nearest_cp) { + dist_nearest_cp = dist_ego2cp; } } } + if (!dist_nearest_cp) { + return {}; + } - // Check if stop is required - if (!nearest_stop_info) { + const auto decided_stop_pose_opt = + calcStopPose(ego_path, dist_nearest_cp.value(), default_stop_pose_opt); + if (!decided_stop_pose_opt.has_value()) { return {}; } + return createStopFactor(decided_stop_pose_opt.value(), stop_factor_points); +} + +std::optional CrosswalkModule::calcStopPose( + const PathWithLaneId & ego_path, double dist_nearest_cp, + const std::optional & default_stop_pose_opt) +{ + struct StopCandidate + { + geometry_msgs::msg::Pose pose; + double dist; + }; + + const auto & p = planner_param_; + const auto base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m; + const auto & ego_pos = planner_data_->current_odometry->pose.position; + const double ego_vel_non_nega = std::max(0.0, planner_data_->current_velocity->twist.linear.x); + const double ego_acc = planner_data_->current_acceleration->accel.accel.linear.x; - // Check if the ego should stop at the stop line or the other points. - const bool stop_at_stop_line = - dist_ego_to_stop < nearest_stop_info->second && - nearest_stop_info->second < dist_ego_to_stop + planner_param_.far_object_threshold; + const auto default_stop_opt = [&]() -> std::optional { + if (!default_stop_pose_opt.has_value()) return std::nullopt; + return StopCandidate{ + default_stop_pose_opt.value(), + calcSignedArcLength(ego_path.points, ego_pos, default_stop_pose_opt->position)}; + }(); + + const auto ped_stop_pref_opt = [&]() -> std::optional { + const double dist = + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_preferred; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist}; + }(); - if (stop_at_stop_line) { - // Stop at the stop line - if (default_stop_pose) { - return createStopFactor(*default_stop_pose, stop_factor_points); + const auto without_acc_pref_stop_opt = [&]() -> std::optional { + if (!ped_stop_pref_opt.has_value()) { + RCLCPP_INFO(logger_, "Failure to calculate pref_stop."); + return std::nullopt; + } else if ( + default_stop_opt.has_value() && ped_stop_pref_opt->dist > default_stop_opt->dist && + ped_stop_pref_opt->dist < default_stop_opt->dist + planner_param_.far_object_threshold) { + return default_stop_opt; + } else { + return ped_stop_pref_opt; } - } else { - const auto stop_pose = calcLongitudinalOffsetPose( - ego_path.points, nearest_stop_info->first, - -base_link2front - planner_param_.stop_distance_from_object); - if (stop_pose) { - return createStopFactor(*stop_pose, stop_factor_points); + }(); + + const auto ped_stop_limit = [&]() -> std::optional { + const double dist = + dist_nearest_cp - base_link2front - planner_param_.stop_distance_from_object_limit; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist}; + }(); + if (!ped_stop_limit.has_value()) { + RCLCPP_WARN( + logger_, + "Stop is canceled. " + "Failure to calculate stop_pose agaist the nearest pedestrian with a limit margin."); + return std::nullopt; + } + + const auto weak_brk_stop = [&]() -> std::optional { + const auto dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel_non_nega, 0.0, ego_acc, p.min_acc_preferred, 10.0, p.min_jerk_preferred); + if (!dist_opt.has_value()) return std::nullopt; + const auto pose_opt = calcLongitudinalOffsetPose(ego_path.points, ego_pos, dist_opt.value()); + if (!pose_opt.has_value()) return std::nullopt; + return StopCandidate{pose_opt.value(), dist_opt.value()}; + }(); + if (!weak_brk_stop.has_value()) { + RCLCPP_ERROR(logger_, "Failure to calculate braking distance. Stop will be canceled."); + return std::nullopt; + } + + const auto selected_stop = [&]() { + if ( + without_acc_pref_stop_opt.has_value() && + weak_brk_stop->dist < without_acc_pref_stop_opt->dist) { + return without_acc_pref_stop_opt.value(); + } else if (weak_brk_stop->dist < ped_stop_limit->dist) { + return weak_brk_stop.value(); + } else { + return ped_stop_limit.value(); } + }(); + + const double strong_brk_dist = [&]() { + const auto strong_brk_dist_opt = autoware::motion_utils::calcDecelDistWithJerkAndAccConstraints( + ego_vel_non_nega, 0.0, ego_acc, p.min_acc_for_no_stop_decision, 10.0, + p.min_jerk_for_no_stop_decision); + return strong_brk_dist_opt ? strong_brk_dist_opt.value() : 0.0; + }(); + if (selected_stop.dist < strong_brk_dist - p.overrun_threshold_length_for_no_stop_decision) { + RCLCPP_INFO( + logger_, + "Abandon to stop. " + "Can not stop against the nearest pedestrian with a specified deceleration. " + "dist to stop: %f, braking distance: %f", + selected_stop.dist, strong_brk_dist); + return std::nullopt; } - return {}; + + return std::make_optional(selected_stop.pose); } std::pair CrosswalkModule::getAttentionRange( 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 be2a7d199c666..cd4b8eb46af6c 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 @@ -112,10 +112,13 @@ class CrosswalkModule : public SceneModuleInterface { bool show_processing_time; // param for stop position - double stop_distance_from_object; + double stop_distance_from_object_preferred; + double stop_distance_from_object_limit; double stop_distance_from_crosswalk; double far_object_threshold; double stop_position_threshold; + double min_acc_preferred; + double min_jerk_preferred; // param for restart suppression double min_dist_to_stop_for_restart_suppression; double max_dist_to_stop_for_restart_suppression; @@ -140,10 +143,9 @@ class CrosswalkModule : public SceneModuleInterface std::vector ego_pass_later_margin_y; double ego_pass_later_additional_margin; double ego_min_assumed_speed; - double max_offset_to_crosswalk_for_yield; double min_acc_for_no_stop_decision; - double max_jerk_for_no_stop_decision; double min_jerk_for_no_stop_decision; + double overrun_threshold_length_for_no_stop_decision; double stop_object_velocity; double min_object_velocity; bool disable_yield_for_new_stopped_object; @@ -352,6 +354,10 @@ class CrosswalkModule : public SceneModuleInterface const PathWithLaneId & ego_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk) const; + std::optional calcStopPose( + const PathWithLaneId & ego_path, double dist_nearest_cp, + const std::optional & default_stop_pose_opt); + std::optional checkStopForCrosswalkUsers( const PathWithLaneId & ego_path, const PathWithLaneId & sparse_resample_path, const geometry_msgs::msg::Point & first_path_point_on_crosswalk,