diff --git a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp index 83514127b0a8e..58bf36d978cea 100644 --- a/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp +++ b/planning/behavior_path_avoidance_module/test/test_behavior_path_planner_node_interface.cpp @@ -62,8 +62,6 @@ std::shared_ptr generateNode() behavior_path_planner_dir + "/config/behavior_path_planner.param.yaml", behavior_path_planner_dir + "/config/drivable_area_expansion.param.yaml", behavior_path_planner_dir + "/config/scene_module_manager.param.yaml", - ament_index_cpp::get_package_share_directory("behavior_path_planner") + - "/config/lane_change/lane_change.param.yaml", ament_index_cpp::get_package_share_directory("behavior_path_avoidance_module") + "/config/avoidance.param.yaml"}); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 6f34bc79fe5f1..debeef5ec573f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -17,12 +17,55 @@ #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include + #include +#include #include namespace behavior_path_planner { +struct LateralAccelerationMap +{ + std::vector base_vel; + std::vector base_min_acc; + std::vector base_max_acc; + + void add(const double velocity, const double min_acc, const double max_acc) + { + if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { + return; + } + + size_t idx = 0; + for (size_t i = 0; i < base_vel.size(); ++i) { + if (velocity < base_vel.at(i)) { + break; + } + idx = i + 1; + } + + base_vel.insert(base_vel.begin() + idx, velocity); + base_min_acc.insert(base_min_acc.begin() + idx, min_acc); + base_max_acc.insert(base_max_acc.begin() + idx, max_acc); + } + + std::pair find(const double velocity) const + { + if (!base_vel.empty() && velocity < base_vel.front()) { + return std::make_pair(base_min_acc.front(), base_max_acc.front()); + } + if (!base_vel.empty() && velocity > base_vel.back()) { + return std::make_pair(base_min_acc.back(), base_max_acc.back()); + } + + const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); + const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); + + return std::make_pair(min_acc, max_acc); + } +}; struct LaneChangeCancelParameters { @@ -33,6 +76,7 @@ struct LaneChangeCancelParameters double max_lateral_jerk{10.0}; double overhang_tolerance{0.0}; }; + struct LaneChangeParameters { // trajectory generation @@ -41,6 +85,15 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // lane change parameters + double backward_length_buffer_for_end_of_lane; + double backward_length_buffer_for_blocking_object; + double lane_changing_lateral_jerk{0.5}; + double minimum_lane_changing_velocity{5.6}; + double lane_change_prepare_duration{4.0}; + double lane_change_finish_judge_buffer{3.0}; + LateralAccelerationMap lane_change_lat_acc_map; + // parked vehicle double object_check_min_road_shoulder_width{0.5}; double object_shiftable_ratio_threshold{0.6}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 12e6da00b2f36..f99ed043eb5dd 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -54,17 +54,21 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection = 0.0); + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params); + const LaneChangeParameters & lane_change_parameters); double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters); double calcLaneChangingAcceleration( const double initial_lane_changing_velocity, const double max_path_velocity, @@ -130,7 +134,7 @@ double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & curent_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction); + const LaneChangeParameters & lane_change_parameters, const Direction direction); lanelet::ConstLanelets getBackwardLanelets( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, @@ -150,7 +154,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution); + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution); bool isParkedObject( const PathWithLaneId & path, const RouteHandler & route_handler, 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 907c5226c3908..46057031afd8a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -238,39 +238,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.min_acc = declare_parameter("normal.min_acc"); p.max_acc = declare_parameter("normal.max_acc"); - // lane change parameters - p.backward_length_buffer_for_end_of_lane = - declare_parameter("lane_change.backward_length_buffer_for_end_of_lane"); - p.backward_length_buffer_for_blocking_object = - declare_parameter("lane_change.backward_length_buffer_for_blocking_object"); - p.lane_changing_lateral_jerk = - declare_parameter("lane_change.lane_changing_lateral_jerk"); - p.lane_change_prepare_duration = declare_parameter("lane_change.prepare_duration"); - p.minimum_lane_changing_velocity = - declare_parameter("lane_change.minimum_lane_changing_velocity"); - p.minimum_lane_changing_velocity = - std::min(p.minimum_lane_changing_velocity, p.max_acc * p.lane_change_prepare_duration); - p.lane_change_finish_judge_buffer = - declare_parameter("lane_change.lane_change_finish_judge_buffer"); - - // lateral acceleration map for lane change - const auto lateral_acc_velocity = - declare_parameter>("lane_change.lateral_acceleration.velocity"); - const auto min_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.min_values"); - const auto max_lateral_acc = - declare_parameter>("lane_change.lateral_acceleration.max_values"); - if ( - lateral_acc_velocity.size() != min_lateral_acc.size() || - lateral_acc_velocity.size() != max_lateral_acc.size()) { - RCLCPP_ERROR(get_logger(), "Lane change lateral acceleration map has invalid size."); - exit(EXIT_FAILURE); - } - for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { - p.lane_change_lat_acc_map.add( - lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); - } - p.backward_length_buffer_for_end_of_pull_over = declare_parameter("backward_length_buffer_for_end_of_pull_over"); p.backward_length_buffer_for_end_of_pull_out = @@ -297,10 +264,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); - if (p.backward_length_buffer_for_end_of_lane < 1.0) { - RCLCPP_WARN_STREAM( - get_logger(), "Lane change buffer must be more than 1 meter. Modifying the buffer."); - } return p; } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index cec9d5013d929..101dd43919361 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -148,8 +148,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - const auto & common_parameters = module_type_->getCommonParam(); - const auto threshold = common_parameters.backward_length_buffer_for_end_of_lane; + const auto threshold = module_type_->getLaneChangeParam().backward_length_buffer_for_end_of_lane; const auto status = module_type_->getLaneChangeStatus(); if (module_type_->isNearEndOfCurrentLanes(status.current_lanes, status.target_lanes, threshold)) { log_warn_throttled("Lane change path is unsafe but near end of lane. Continue lane change."); @@ -432,8 +431,8 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const auto & common_parameter = module_type_->getCommonParam(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameter, shift_intervals, common_parameter.backward_length_buffer_for_end_of_lane); + const double next_lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); const double & nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double & nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double & base_to_front = common_parameter.base_link2front; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index ed40476723dba..658ffb06c25fe 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -141,6 +141,49 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) p.rss_params_for_stuck.lateral_distance_max_threshold = getOrDeclareParameter( *node, parameter("safety_check.stuck.lateral_distance_max_threshold")); + // lane change parameters + const auto max_acc = getOrDeclareParameter(*node, "normal.max_acc"); + p.backward_length_buffer_for_end_of_lane = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); + p.backward_length_buffer_for_blocking_object = + getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.lane_changing_lateral_jerk = + getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); + p.lane_change_prepare_duration = + getOrDeclareParameter(*node, parameter("prepare_duration")); + p.minimum_lane_changing_velocity = + getOrDeclareParameter(*node, parameter("minimum_lane_changing_velocity")); + p.minimum_lane_changing_velocity = + std::min(p.minimum_lane_changing_velocity, max_acc * p.lane_change_prepare_duration); + p.lane_change_finish_judge_buffer = + getOrDeclareParameter(*node, parameter("lane_change_finish_judge_buffer")); + + if (p.backward_length_buffer_for_end_of_lane < 1.0) { + RCLCPP_WARN_STREAM( + node->get_logger().get_child(name()), + "Lane change buffer must be more than 1 meter. Modifying the buffer."); + } + + // lateral acceleration map for lane change + const auto lateral_acc_velocity = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.velocity")); + const auto min_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.min_values")); + const auto max_lateral_acc = + getOrDeclareParameter>(*node, parameter("lateral_acceleration.max_values")); + if ( + lateral_acc_velocity.size() != min_lateral_acc.size() || + lateral_acc_velocity.size() != max_lateral_acc.size()) { + RCLCPP_ERROR( + node->get_logger().get_child(name()), + "Lane change lateral acceleration map has invalid size."); + exit(EXIT_FAILURE); + } + for (size_t i = 0; i < lateral_acc_velocity.size(); ++i) { + p.lane_change_lat_acc_map.add( + lateral_acc_velocity.at(i), min_lateral_acc.at(i), max_lateral_acc.at(i)); + } + // target object { const std::string ns = "lane_change.target_object."; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 423ffc48dd6cd..ebb41ae63dcbe 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -215,9 +215,8 @@ void NormalLaneChange::insertStopPoint( } const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -233,7 +232,7 @@ void NormalLaneChange::insertStopPoint( distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); } - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; @@ -294,15 +293,14 @@ void NormalLaneChange::insertStopPoint( if (distance_to_ego_lane_obj < distance_to_terminal) { // consider rss distance when the LC need to avoid obstacles const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); - const double lane_change_buffer_for_blocking_object = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, - getCommonParam().backward_length_buffer_for_blocking_object, 0.0); + const double lane_change_buffer_for_blocking_object = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - - getCommonParam().backward_length_buffer_for_blocking_object - rss_dist - + lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist - getCommonParam().base_link2front; // If the target lane in the lane change section is blocked by a stationary obstacle, there @@ -498,9 +496,8 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & current_pose = getEgoPose(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const auto lane_change_buffer = utils::calcMinimumLaneChangeLength( - planner_data_->parameters, shift_intervals, - getCommonParam().backward_length_buffer_for_end_of_lane); + const auto lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -519,7 +516,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; const double dist_to_lane_change_end = utils::getSignedDistance( current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); - double finish_judge_buffer = planner_data_->parameters.lane_change_finish_judge_buffer; + double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer const double ego_velocity = getEgoVelocity(); @@ -560,7 +557,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const planner_data_->parameters.ego_nearest_yaw_threshold); const double ego_velocity = - std::max(getEgoVelocity(), planner_data_->parameters.minimum_lane_changing_velocity); + std::max(getEgoVelocity(), lane_change_parameters_->minimum_lane_changing_velocity); const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; @@ -664,10 +661,10 @@ std::pair NormalLaneChange::calcCurrentMinMaxAcceleration() cons prev_module_path_.points.at(ego_seg_idx).point.longitudinal_velocity_mps; // calculate minimum and maximum acceleration - const auto min_acc = - utils::lane_change::calcMinimumAcceleration(getEgoVelocity(), vehicle_min_acc, p); + const auto min_acc = utils::lane_change::calcMinimumAcceleration( + getEgoVelocity(), vehicle_min_acc, *lane_change_parameters_); const auto max_acc = utils::lane_change::calcMaximumAcceleration( - getEgoVelocity(), max_path_velocity, vehicle_max_acc, p); + getEgoVelocity(), max_path_velocity, vehicle_max_acc, *lane_change_parameters_); return {min_acc, max_acc}; } @@ -678,8 +675,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength( const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); return utils::lane_change::calcMaximumLaneChangeLength( - std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(), - shift_intervals, max_acc); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()), + *lane_change_parameters_, shift_intervals, max_acc); } std::vector NormalLaneChange::sampleLongitudinalAccValues( @@ -746,7 +743,6 @@ std::vector NormalLaneChange::sampleLongitudinalAccValues( std::vector NormalLaneChange::calcPrepareDuration( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const { - const auto & common_parameters = planner_data_->parameters; const auto base_link2front = planner_data_->parameters.base_link2front; const auto threshold = lane_change_parameters_->min_length_for_turn_signal_activation + base_link2front; @@ -754,7 +750,7 @@ std::vector NormalLaneChange::calcPrepareDuration( std::vector prepare_durations; constexpr double step = 0.5; - for (double duration = common_parameters.lane_change_prepare_duration; duration >= 0.0; + for (double duration = lane_change_parameters_->lane_change_prepare_duration; duration >= 0.0; duration -= step) { prepare_durations.push_back(duration); if (!isNearEndOfCurrentLanes(current_lanes, target_lanes, threshold)) { @@ -1024,12 +1020,11 @@ bool NormalLaneChange::hasEnoughLength( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto overall_graphs_ptr = route_handler.getOverallGraphPtr(); - const auto & common_parameters = planner_data_->parameters; const double lane_change_length = path.info.length.sum(); const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(target_lanes.back(), direction); - double minimum_lane_change_length_to_preferred_lane = utils::calcMinimumLaneChangeLength( - common_parameters, shift_intervals, common_parameters.backward_length_buffer_for_end_of_lane); + double minimum_lane_change_length_to_preferred_lane = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1125,7 +1120,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto backward_path_length = common_parameters.backward_path_length; const auto forward_path_length = common_parameters.forward_path_length; - const auto minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; // get velocity @@ -1137,12 +1133,12 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(current_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); - const double next_lane_change_buffer = utils::calcMinimumLaneChangeLength( - common_parameters, route_handler.getLateralIntervalsToPreferredLane(target_lanes.back()), - common_parameters.backward_length_buffer_for_end_of_lane); + const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); + const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, + route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1219,7 +1215,7 @@ bool NormalLaneChange::getLaneChangePaths( // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameters.lane_change_lat_acc_map.find(initial_lane_changing_velocity); + lane_change_parameters_->lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; @@ -1238,7 +1234,7 @@ bool NormalLaneChange::getLaneChangePaths( }; const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( - shift_length, common_parameters.lane_changing_lateral_jerk, lateral_acc); + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, lateral_acc); const double longitudinal_acc_on_lane_changing = utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, @@ -1264,8 +1260,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto num = std::abs(route_handler.getNumLaneToPreferredLane(target_lanes.back(), direction)); const double backward_buffer = - num == 0 ? 0.0 : common_parameters.backward_length_buffer_for_end_of_lane; - const double finish_judge_buffer = common_parameters.lane_change_finish_judge_buffer; + num == 0 ? 0.0 : lane_change_parameters_->backward_length_buffer_for_end_of_lane; + const double finish_judge_buffer = + lane_change_parameters_->lane_change_finish_judge_buffer; if ( s_start + lane_changing_length + finish_judge_buffer + backward_buffer + next_lane_change_buffer > @@ -1536,7 +1533,7 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) const { - const auto threshold = planner_data_->parameters.backward_length_buffer_for_end_of_lane; + const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; return isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && isAbleToStopSafely() && is_object_coming_from_rear; } @@ -1546,7 +1543,7 @@ bool NormalLaneChange::calcAbortPath() const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); const auto current_velocity = - std::max(common_param.minimum_lane_changing_velocity, getEgoVelocity()); + std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; const auto reference_lanelets = selected_path.info.current_lanes; @@ -1557,8 +1554,8 @@ bool NormalLaneChange::calcAbortPath() const auto direction = getDirection(); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane( selected_path.info.current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1605,8 +1602,8 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - *route_handler, reference_lanelets, current_pose, abort_return_dist, common_param, - direction)) { + *route_handler, reference_lanelets, current_pose, abort_return_dist, + *lane_change_parameters_, direction)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -1629,7 +1626,8 @@ bool NormalLaneChange::calcAbortPath() const auto lateral_jerk = behavior_path_planner::PathShifter::calcJerkFromLatLonDistance( shift_line.end_shift_length, abort_start_dist, current_velocity); path_shifter.setVelocity(current_velocity); - const auto lateral_acc_range = common_param.lane_change_lat_acc_map.find(current_velocity); + const auto lateral_acc_range = + lane_change_parameters_->lane_change_lat_acc_map.find(current_velocity); const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); @@ -1662,7 +1660,7 @@ bool NormalLaneChange::calcAbortPath() PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); ref.points.back().point.longitudinal_velocity_mps = std::min( ref.points.back().point.longitudinal_velocity_mps, - static_cast(common_param.minimum_lane_changing_velocity)); + static_cast(lane_change_parameters_->minimum_lane_changing_velocity)); return ref; }); @@ -1707,7 +1705,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const double & time_resolution = lane_change_parameters_->prediction_time_resolution; const auto ego_predicted_path = utils::lane_change::convertToPredictedPath( - lane_change_path, current_twist, current_pose, common_parameters, time_resolution); + lane_change_path, current_twist, current_pose, common_parameters, *lane_change_parameters_, + time_resolution); const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); @@ -1818,10 +1817,9 @@ bool NormalLaneChange::isVehicleStuck( : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); - const double lane_change_buffer = utils::calcMinimumLaneChangeLength( - getCommonParam(), shift_intervals, getCommonParam().backward_length_buffer_for_end_of_lane, - 0.0); - const double stop_point_buffer = getCommonParam().backward_length_buffer_for_end_of_lane; + const double lane_change_buffer = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0); + const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { return true; @@ -1844,7 +1842,7 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan const auto [min_acc, max_acc] = calcCurrentMinMaxAcceleration(); const auto max_lane_change_length = calcMaximumLaneChangeLength(current_lanes.back(), max_acc); const auto rss_dist = calcRssDistance( - 0.0, planner_data_->parameters.minimum_lane_changing_velocity, + 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); // It is difficult to define the detection range. If it is too short, the stuck will not be diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 66182454ecf85..8ba820231d7cc 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -67,17 +67,43 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } +double calcMinimumLaneChangeLength( + const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, + const double length_to_intersection) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const double & vel = lane_change_parameters.minimum_lane_changing_velocity; + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); + const double & max_lateral_acc = lat_acc.second; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; + + double accumulated_length = length_to_intersection; + for (const auto & shift_interval : shift_intervals) { + const double t = + PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); + accumulated_length += vel * t + finish_judge_buffer; + } + accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); + + return accumulated_length; +} + double calcMaximumLaneChangeLength( - const double current_velocity, const BehaviorPathPlannerParameters & common_param, + const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) { if (shift_intervals.empty()) { return 0.0; } - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & t_prepare = common_param.lane_change_prepare_duration; + const double & finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; + const double & lateral_jerk = lane_change_parameters.lane_changing_lateral_jerk; + const double & t_prepare = lane_change_parameters.lane_change_prepare_duration; double vel = current_velocity; double accumulated_length = 0.0; @@ -87,7 +113,7 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_prepare; // lane changing section - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); + const auto lat_acc = lane_change_parameters.lane_change_lat_acc_map.find(vel); const double t_lane_changing = PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, lat_acc.second); const double lane_changing_length = @@ -97,26 +123,26 @@ double calcMaximumLaneChangeLength( vel = vel + max_acc * t_lane_changing; } accumulated_length += - common_param.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); + lane_change_parameters.backward_length_buffer_for_end_of_lane * (shift_intervals.size() - 1.0); return accumulated_length; } double calcMinimumAcceleration( const double current_velocity, const double min_longitudinal_acc, - const BehaviorPathPlannerParameters & params) + const LaneChangeParameters & lane_change_parameters) { - const double min_lane_changing_velocity = params.minimum_lane_changing_velocity; - const double prepare_duration = params.lane_change_prepare_duration; + const double min_lane_changing_velocity = lane_change_parameters.minimum_lane_changing_velocity; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (min_lane_changing_velocity - current_velocity) / prepare_duration; return std::clamp(acc, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); } double calcMaximumAcceleration( const double current_velocity, const double current_max_velocity, - const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) + const double max_longitudinal_acc, const LaneChangeParameters & lane_change_parameters) { - const double prepare_duration = params.lane_change_prepare_duration; + const double prepare_duration = lane_change_parameters.lane_change_prepare_duration; const double acc = (current_max_velocity - current_velocity) / prepare_duration; return std::clamp(acc, 0.0, max_longitudinal_acc); } @@ -601,12 +627,12 @@ double getLateralShift(const LaneChangePath & path) bool hasEnoughLengthToLaneChangeAfterAbort( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const double abort_return_dist, - const BehaviorPathPlannerParameters & common_param, const Direction direction) + const LaneChangeParameters & lane_change_parameters, const Direction direction) { const auto shift_intervals = route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction); - const double minimum_lane_change_length = utils::calcMinimumLaneChangeLength( - common_param, shift_intervals, common_param.backward_length_buffer_for_end_of_lane); + const double minimum_lane_change_length = + calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -765,7 +791,8 @@ std::optional getLaneChangeTargetLane( std::vector convertToPredictedPath( const LaneChangePath & lane_change_path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const BehaviorPathPlannerParameters & common_parameters, const double resolution) + const BehaviorPathPlannerParameters & common_parameters, + const LaneChangeParameters & lane_change_parameters, const double resolution) { if (lane_change_path.path.points.empty()) { return {}; @@ -776,7 +803,8 @@ std::vector convertToPredictedPath( const auto lane_changing_acc = lane_change_path.info.longitudinal_acceleration.lane_changing; const auto duration = lane_change_path.info.duration.sum(); const auto prepare_time = lane_change_path.info.duration.prepare; - const auto & minimum_lane_changing_velocity = common_parameters.minimum_lane_changing_velocity; + const auto & minimum_lane_changing_velocity = + lane_change_parameters.minimum_lane_changing_velocity; const auto nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, vehicle_pose, common_parameters.ego_nearest_dist_threshold, @@ -1065,7 +1093,8 @@ std::optional createPolygon( } ExtendedPredictedObject transform( - const PredictedObject & object, const BehaviorPathPlannerParameters & common_parameters, + const PredictedObject & object, + [[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters, const LaneChangeParameters & lane_change_parameters) { ExtendedPredictedObject extended_object; @@ -1078,7 +1107,7 @@ ExtendedPredictedObject transform( const auto & time_resolution = lane_change_parameters.prediction_time_resolution; const auto & check_at_prepare_phase = lane_change_parameters.enable_prepare_segment_collision_check; - const auto & prepare_duration = common_parameters.lane_change_prepare_duration; + const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration; const auto & velocity_threshold = lane_change_parameters.prepare_segment_ignore_object_velocity_thresh; const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration; diff --git a/planning/behavior_path_planner/test/test_lane_change_utils.cpp b/planning/behavior_path_planner/test/test_lane_change_utils.cpp index 3268bd7ec468e..477af32893b0b 100644 --- a/planning/behavior_path_planner/test/test_lane_change_utils.cpp +++ b/planning/behavior_path_planner/test/test_lane_change_utils.cpp @@ -11,6 +11,7 @@ // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp" #include "behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include @@ -41,7 +42,7 @@ TEST(BehaviorPathPlanningLaneChangeUtilsTest, projectCurrentPoseToTarget) TEST(BehaviorPathPlanningLaneChangeUtilsTest, TESTLateralAccelerationMap) { - LateralAccelerationMap lat_acc_map; + behavior_path_planner::LateralAccelerationMap lat_acc_map; lat_acc_map.add(0.0, 0.2, 0.315); lat_acc_map.add(3.0, 0.2, 0.315); lat_acc_map.add(5.0, 0.2, 0.315); diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp index 7832dd4e53cfc..5bdd0a2f3f88d 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp @@ -15,7 +15,6 @@ #ifndef BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER_COMMON__PARAMETERS_HPP_ -#include #include #include @@ -32,47 +31,6 @@ struct ModuleConfigParameters uint8_t max_module_size{0}; }; -struct LateralAccelerationMap -{ - std::vector base_vel; - std::vector base_min_acc; - std::vector base_max_acc; - - void add(const double velocity, const double min_acc, const double max_acc) - { - if (base_vel.size() != base_min_acc.size() || base_vel.size() != base_max_acc.size()) { - return; - } - - size_t idx = 0; - for (size_t i = 0; i < base_vel.size(); ++i) { - if (velocity < base_vel.at(i)) { - break; - } - idx = i + 1; - } - - base_vel.insert(base_vel.begin() + idx, velocity); - base_min_acc.insert(base_min_acc.begin() + idx, min_acc); - base_max_acc.insert(base_max_acc.begin() + idx, max_acc); - } - - std::pair find(const double velocity) const - { - if (!base_vel.empty() && velocity < base_vel.front()) { - return std::make_pair(base_min_acc.front(), base_max_acc.front()); - } - if (!base_vel.empty() && velocity > base_vel.back()) { - return std::make_pair(base_min_acc.back(), base_max_acc.back()); - } - - const double min_acc = interpolation::lerp(base_vel, base_min_acc, velocity); - const double max_acc = interpolation::lerp(base_vel, base_max_acc, velocity); - - return std::make_pair(min_acc, max_acc); - } -}; - struct BehaviorPathPlannerParameters { bool verbose; @@ -81,8 +39,6 @@ struct BehaviorPathPlannerParameters double backward_path_length; double forward_path_length; - double backward_length_buffer_for_end_of_lane; - double backward_length_buffer_for_blocking_object; double backward_length_buffer_for_end_of_pull_over; double backward_length_buffer_for_end_of_pull_out; @@ -90,13 +46,6 @@ struct BehaviorPathPlannerParameters double min_acc; double max_acc; - // lane change parameters - double lane_changing_lateral_jerk{0.5}; - double minimum_lane_changing_velocity{5.6}; - double lane_change_prepare_duration{4.0}; - double lane_change_finish_judge_buffer{3.0}; - LateralAccelerationMap lane_change_lat_acc_map; - double minimum_pull_over_length; double minimum_pull_out_length; double drivable_area_resolution; diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp index 3a7d346849e16..e2c9fd1e332a2 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/utils.hpp @@ -333,10 +333,6 @@ lanelet::ConstLanelets calcLaneAroundPose( bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold); -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection = 0.0); - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); diff --git a/planning/behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner_common/src/utils/utils.cpp index 88c7532b0ad72..7c3a5883989fb 100644 --- a/planning/behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner_common/src/utils/utils.cpp @@ -1515,31 +1515,6 @@ bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_thre return true; } -double calcMinimumLaneChangeLength( - const BehaviorPathPlannerParameters & common_param, const std::vector & shift_intervals, - const double backward_buffer, const double length_to_intersection) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const double & vel = common_param.minimum_lane_changing_velocity; - const auto lat_acc = common_param.lane_change_lat_acc_map.find(vel); - const double & max_lateral_acc = lat_acc.second; - const double & lateral_jerk = common_param.lane_changing_lateral_jerk; - const double & finish_judge_buffer = common_param.lane_change_finish_judge_buffer; - - double accumulated_length = length_to_intersection; - for (const auto & shift_interval : shift_intervals) { - const double t = - PathShifter::calcShiftTimeFromJerk(shift_interval, lateral_jerk, max_lateral_acc); - accumulated_length += vel * t + finish_judge_buffer; - } - accumulated_length += backward_buffer * (shift_intervals.size() - 1.0); - - return accumulated_length; -} - lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler) {