diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index 6fc136c512689..32902fa838b1c 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -7,6 +7,7 @@ attention_area_angle_threshold: 0.785 use_intersection_area: false default_stopline_margin: 3.0 + second_pass_judge_line_margin: 1.0 stopline_overshoot_margin: 0.5 path_interpolation_ds: 0.1 max_accel: -2.8 @@ -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/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 e859b15b345b8..6e4e72c19d9d2 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -53,6 +53,8 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".common.use_intersection_area"); ip.common.default_stopline_margin = getOrDeclareParameter(node, ns + ".common.default_stopline_margin"); + ip.common.second_pass_judge_line_margin = + getOrDeclareParameter(node, ns + ".common.second_pass_judge_line_margin"); ip.common.stopline_overshoot_margin = getOrDeclareParameter(node, ns + ".common.stopline_overshoot_margin"); ip.common.path_interpolation_ds = @@ -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 1bc7bfe04d271..1380dc7979334 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,49 @@ 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_) || - is_permanent_go_) { - // is_go_out_: previous RTC approval - // activated_: current RTC approval + + 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 && is_over_2nd_pass_judge_line && was_safe) || is_permanent_go_) { + /* + * 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,6 +1338,8 @@ 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 bool has_collision = checkCollision( *path, &target_objects, path_lanelets, closest_idx, std::min(occlusion_stopline_idx, path->points.size() - 1), time_to_restart, @@ -1677,6 +1708,7 @@ std::optional IntersectionModule::generateIntersectionSto const double max_jerk = planner_param_.common.max_jerk; const double delay_response_time = planner_param_.common.delay_response_time; const double peeking_offset = planner_param_.occlusion.peeking_offset; + const double second_pass_judge_line_margin = planner_param_.common.second_pass_judge_line_margin; const auto first_attention_area = first_attention_lane.polygon3d(); const auto first_attention_lane_centerline = first_attention_lane.centerline2d(); @@ -1814,10 +1846,13 @@ std::optional IntersectionModule::generateIntersectionSto 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 = second_attention_stopline_ip_int - std::ceil(braking_dist / ds) - + std::ceil(second_pass_judge_line_margin / ds); 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 { diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index 38feada725872..f8a274cb7a7de 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}; @@ -339,6 +342,7 @@ class IntersectionModule : public SceneModuleInterface double attention_area_angle_threshold; bool use_intersection_area; double default_stopline_margin; + double second_pass_judge_line_margin; double stopline_overshoot_margin; double path_interpolation_ds; double max_accel; @@ -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_;