diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml index be8e822b31d5c..fde55dc7a6c55 100644 --- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml +++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml @@ -12,6 +12,9 @@ use_intersection_area: false path_interpolation_ds: 0.1 # [m] consider_wrong_direction_vehicle: false + max_accel: -2.8 + max_jerk: -5.0 + delay_response_time: 0.5 stuck_vehicle: turn_direction: left: true diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp index 0b4131b55ed90..a65e06eedcdf0 100644 --- a/planning/behavior_velocity_intersection_module/src/manager.cpp +++ b/planning/behavior_velocity_intersection_module/src/manager.cpp @@ -62,6 +62,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node) getOrDeclareParameter(node, ns + ".common.path_interpolation_ds"); ip.common.consider_wrong_direction_vehicle = getOrDeclareParameter(node, ns + ".common.consider_wrong_direction_vehicle"); + ip.common.max_accel = getOrDeclareParameter(node, ns + ".common.max_accel"); + ip.common.max_jerk = getOrDeclareParameter(node, ns + ".common.max_jerk"); + ip.common.delay_response_time = + getOrDeclareParameter(node, ns + ".common.delay_response_time"); ip.stuck_vehicle.turn_direction.left = getOrDeclareParameter(node, ns + ".stuck_vehicle.turn_direction.left"); diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp index 1bbff9ecaf048..4def152567b32 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp @@ -987,7 +987,8 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail( const auto intersection_stop_lines_opt = util::generateIntersectionStopLines( first_conflicting_area, dummy_first_attention_area, planner_data_, interpolated_path_info, planner_param_.stuck_vehicle.use_stuck_stopline, planner_param_.common.stop_line_margin, - peeking_offset, path); + planner_param_.common.max_accel, planner_param_.common.max_jerk, + planner_param_.common.delay_response_time, peeking_offset, path); if (!intersection_stop_lines_opt) { return IntersectionModule::Indecisive{"failed to generate intersection_stop_lines"}; } diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp index c604f01c7a86b..764f5bd7fe058 100644 --- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp +++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp @@ -58,6 +58,9 @@ class IntersectionModule : public SceneModuleInterface bool use_intersection_area; bool consider_wrong_direction_vehicle; double path_interpolation_ds; + double max_accel; + double max_jerk; + double delay_response_time; } common; struct StuckVehicle { diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp index d3b2ca3baceb4..27310f2129937 100644 --- a/planning/behavior_velocity_intersection_module/src/util.cpp +++ b/planning/behavior_velocity_intersection_module/src/util.cpp @@ -269,7 +269,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double max_accel, + const double max_jerk, const double delay_response_time, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path) { const auto & path_ip = interpolated_path_info.path; @@ -350,11 +351,8 @@ std::optional generateIntersectionStopLines( // (4) pass judge line position on interpolated path const double velocity = planner_data->current_velocity->twist.linear.x; const double acceleration = planner_data->current_acceleration->accel.accel.linear.x; - const double max_stop_acceleration = planner_data->max_stop_acceleration_threshold; - const double max_stop_jerk = planner_data->max_stop_jerk_threshold; - const double delay_response_time = planner_data->delay_response_time; const double braking_dist = planning_utils::calcJudgeLineDistWithJerkLimit( - velocity, acceleration, max_stop_acceleration, max_stop_jerk, delay_response_time); + velocity, acceleration, max_accel, max_jerk, delay_response_time); int pass_judge_ip_int = static_cast(first_footprint_inside_detection_ip) - std::ceil(braking_dist / ds); const auto pass_judge_line_ip = static_cast( diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp index ef658a25fce55..125d3bdfb570a 100644 --- a/planning/behavior_velocity_intersection_module/src/util.hpp +++ b/planning/behavior_velocity_intersection_module/src/util.hpp @@ -70,7 +70,8 @@ std::optional generateIntersectionStopLines( const lanelet::CompoundPolygon3d & first_detection_area, const std::shared_ptr & planner_data, const InterpolatedPathInfo & interpolated_path_info, const bool use_stuck_stopline, - const double stop_line_margin, const double peeking_offset, + const double stop_line_margin, const double peeking_offset, const double max_accel, + const double max_jerk, const double delay_response_time, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path); std::optional getFirstPointInsidePolygon(