diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 9ce619c189146..1e15ee93a7d19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -110,8 +110,9 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length); + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index afb1997d01e74..9cd4759ac5d90 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -143,8 +143,7 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - constexpr double resample_interval{1.0}; - auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + auto path = status_.lane_change_path.path; if (!isValidPath(path)) { status_.is_safe = false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 9b3e65ec92a87..11e91e14a4ac0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -237,7 +238,7 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length); + lane_changing_distance, forward_path_length, lane_changing_speed); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -490,8 +491,9 @@ bool isLaneChangePathSafe( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length) + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -502,7 +504,14 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); s_end = std::min(s_end, goal_arc_coordinates.length); } - return route_handler.getCenterLinePath(target_lanes, s_start, s_end); + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + const auto resample_interval = + std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); + return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); } PathWithLaneId getReferencePathFromTargetLane(