diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 031ddc08fba1d..a8631128d8abd 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -15,6 +15,10 @@ longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 + # longitudinal acceleration + min_longitudinal_acc: -1.0 + max_longitudinal_acc: 1.0 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 827eddbad499d..67aa9b3bfb5ea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -182,8 +182,7 @@ class LaneChangeBase virtual PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const = 0; + const double backward_path_length, const double prepare_length) const = 0; virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 9e262a677d365..6869ad22cae54 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -91,8 +91,7 @@ class NormalLaneChange : public LaneChangeBase PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const override; + const double backward_path_length, const double prepare_length) const override; bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, @@ -128,8 +127,7 @@ class NormalLaneChangeBT : public NormalLaneChange PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const override; + const double backward_path_length, const double prepare_length) const override; std::vector getDrivableLanes() const override; }; 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 2b5e11238383c..8c25b7873d8db 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 @@ -35,6 +35,10 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // acceleration data + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + // collision check bool enable_prepare_segment_collision_check{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; 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 eb81c92eb80a8..bea70e01e239f 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 @@ -55,7 +55,11 @@ double calcLaneChangeResampleInterval( double calcMaximumAcceleration( const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const BehaviorPathPlannerParameters & params); + const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); + +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); @@ -77,19 +81,14 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); -double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, const double lateral_acc, - const double lateral_jerk); - std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, - const LaneChangePhaseInfo lane_change_velocity, - const BehaviorPathPlannerParameters & common_parameter, - const LaneChangeParameters & lane_change_param); + const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity, + const LaneChangePhaseInfo lane_change_time); PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, @@ -97,8 +96,8 @@ PathSafetyStatus isLaneChangePathSafe( const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, - std::unordered_map & debug_data, - const double acceleration = 0.0); + std::unordered_map & debug_data, const double prepare_acc = 0.0, + const double lane_changing_acc = 0.0); bool isObjectIndexIncluded( const size_t & index, const std::vector & dynamic_objects_indices); @@ -172,5 +171,10 @@ CandidateOutput assignToCandidate( boost::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction); + +PredictedPath convertToPredictedPath( + const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, + const size_t nearest_seg_idx, const double duration, const double resolution, + const double prepare_time, const double prepare_acc, const double lane_changing_acc); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 87bced439ce46..f9b1b25c5c428 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -107,11 +107,6 @@ struct FrenetPoint }; // data conversions -PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, - const size_t nearest_seg_idx, const double duration, const double resolution, - const double prepare_time, const double acceleration); - template FrenetPoint convertToFrenetPoint( const T & points, const Point & search_point_geom, const size_t seg_idx) 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 407f2d853f88a..51650803f8be8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -749,6 +749,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // acceleration + p.min_longitudinal_acc = declare_parameter(parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = declare_parameter(parameter("max_longitudinal_acc")); + // collision check p.enable_prepare_segment_collision_check = declare_parameter(parameter("enable_prepare_segment_collision_check")); 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 2c51640be6aa6..e380993cc01c1 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 @@ -371,7 +371,7 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) PathWithLaneId NormalLaneChange::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double current_velocity, const double prepare_velocity) const + const double prepare_length) const { if (current_lanes.empty()) { return PathWithLaneId(); @@ -382,8 +382,6 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); - return prepare_segment; } @@ -406,20 +404,24 @@ bool NormalLaneChange::getLaneChangePaths( const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; + const auto min_longitudinal_acc = + std::max(common_parameter.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto max_longitudinal_acc = + std::min(common_parameter.max_acc, lane_change_parameters_->max_longitudinal_acc); // get velocity const auto current_velocity = getEgoTwist().linear.x; // compute maximum longitudinal deceleration and acceleration - const auto maximum_deceleration = - std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, this]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameter.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); - }); + const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, + &min_longitudinal_acc, &common_parameter, this]() { + const double min_a = (minimum_lane_changing_velocity - current_velocity) / + common_parameter.lane_change_prepare_duration; + return std::clamp( + min_a, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); + }); const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( - prev_module_path_, getEgoPose(), current_velocity, common_parameter); + prev_module_path_, getEgoPose(), current_velocity, max_longitudinal_acc, common_parameter); // get sampling acceleration values const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( @@ -473,9 +475,8 @@ bool NormalLaneChange::getLaneChangePaths( break; } - const auto prepare_segment = getPrepareSegment( - original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length, - current_velocity, prepare_velocity); + auto prepare_segment = getPrepareSegment( + original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { RCLCPP_DEBUG( @@ -502,21 +503,30 @@ bool NormalLaneChange::getLaneChangePaths( const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); - // we assume constant velocity during lane change - const auto lane_changing_velocity = prepare_velocity; + const auto initial_lane_changing_velocity = prepare_velocity; + const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameter.lane_change_lat_acc_map.find(lane_changing_velocity); + common_parameter.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; constexpr double lateral_acc_epsilon = 0.01; for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; lateral_acc += lateral_acc_resolution) { - const auto lane_changing_length = utils::lane_change::calcLaneChangingLength( - lane_changing_velocity, shift_length, lateral_acc, - common_parameter.lane_changing_lateral_jerk); + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); + const double lane_changing_lon_acc = utils::lane_change::calcLaneChangingAcceleration( + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, + sampled_longitudinal_acc); + const auto lane_changing_length = + initial_lane_changing_velocity * lane_changing_time + + 0.5 * lane_changing_lon_acc * lane_changing_time * lane_changing_time; + const auto terminal_lane_changing_velocity = + initial_lane_changing_velocity + lane_changing_lon_acc * lane_changing_time; + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { RCLCPP_DEBUG( @@ -543,7 +553,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_segment = utils::lane_change::getTargetSegment( route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer); + target_lane_length, lane_changing_length, initial_lane_changing_velocity, + next_lane_change_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG( @@ -553,7 +564,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, lane_changing_velocity); + lane_changing_length, initial_lane_changing_velocity); const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( @@ -571,12 +582,15 @@ bool NormalLaneChange::getLaneChangePaths( const auto shift_line = utils::lane_change::getLaneChangingShiftLine( prepare_segment, target_segment, target_lane_reference_path, shift_length); - const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; + const auto lc_velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + + const auto lc_time = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; const auto candidate_path = utils::lane_change::constructCandidatePath( prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, longitudinal_acc, lateral_acc, lc_length, lc_velocity, - common_parameter, *lane_change_parameters_); + target_lanelets, sorted_lane_ids, lane_changing_lon_acc, lateral_acc, lc_length, + lc_velocity, terminal_lane_changing_velocity, lc_time); if (!candidate_path) { RCLCPP_DEBUG( @@ -612,7 +626,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc); + common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc, + lane_changing_lon_acc); if (is_safe) { return true; @@ -960,8 +975,7 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane PathWithLaneId NormalLaneChangeBT::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const + const double backward_path_length, const double prepare_length) const { if (current_lanes.empty()) { return PathWithLaneId(); @@ -977,8 +991,6 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment( PathWithLaneId prepare_segment = getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); - return prepare_segment; } 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 3145bc4ac7394..a6525cebaa19b 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -63,11 +63,10 @@ double calcLaneChangeResampleInterval( double calcMaximumAcceleration( const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const BehaviorPathPlannerParameters & params) + const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) { - const double & max_acc = params.max_acc; if (path.points.empty()) { - return max_acc; + return max_longitudinal_acc; } const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; @@ -80,7 +79,20 @@ double calcMaximumAcceleration( const double & prepare_duration = params.lane_change_prepare_duration; const double acc = (max_path_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, 0.0, max_acc); + return std::clamp(acc, 0.0, max_longitudinal_acc); +} + +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); } void setPrepareVelocity( @@ -171,20 +183,20 @@ std::optional constructCandidatePath( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, - const LaneChangePhaseInfo lane_change_velocity, - const BehaviorPathPlannerParameters & common_parameter, - const LaneChangeParameters & lane_change_param) + const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity, + const LaneChangePhaseInfo lane_change_time) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); path_shifter.addShiftLine(shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration); ShiftedPath shifted_path; // offset front side bool offset_back = false; - const auto lane_changing_velocity = lane_change_velocity.lane_changing; - path_shifter.setVelocity(lane_changing_velocity); + const auto initial_lane_changing_velocity = lane_change_velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); path_shifter.setLateralAccelerationLimit(std::abs(lateral_acceleration)); if (!path_shifter.generate(&shifted_path, offset_back)) { @@ -200,15 +212,8 @@ std::optional constructCandidatePath( candidate_path.acceleration = longitudinal_acceleration; candidate_path.length.prepare = prepare_length; candidate_path.length.lane_changing = lane_changing_length; - candidate_path.duration.prepare = std::invoke([&]() { - const auto duration = prepare_length / lane_change_velocity.prepare; - return std::min(duration, common_parameter.lane_change_prepare_duration); - }); - candidate_path.duration.lane_changing = std::invoke([&]() { - const auto rounding_multiplier = 1.0 / lane_change_param.prediction_time_resolution; - return std::ceil((lane_changing_length / lane_changing_velocity) * rounding_multiplier) / - rounding_multiplier; - }); + candidate_path.duration.prepare = lane_change_time.prepare; + candidate_path.duration.lane_changing = lane_change_time.lane_changing; candidate_path.shift_line = shift_line; candidate_path.reference_lanelets = original_lanelets; candidate_path.target_lanelets = target_lanelets; @@ -220,7 +225,6 @@ std::optional constructCandidatePath( .get_child("constructCandidatePath"), "prepare_length: %f, lane_change: %f", prepare_length, lane_changing_length); - const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = target_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lane_change_end_idx = @@ -238,8 +242,7 @@ std::optional constructCandidatePath( if (i < *lane_change_end_idx) { point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - lane_changing_start_point.point.longitudinal_velocity_mps); + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); continue; } const auto nearest_idx = @@ -316,7 +319,7 @@ PathSafetyStatus isLaneChangePathSafe( const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, std::unordered_map & debug_data, - const double acceleration) + const double prepare_acc, const double lane_changing_acc) { PathSafetyStatus path_safety_status; @@ -342,9 +345,9 @@ PathSafetyStatus isLaneChangePathSafe( path.points, current_pose, common_parameter.ego_nearest_dist_threshold, common_parameter.ego_nearest_yaw_threshold); - const auto ego_predicted_path = utils::convertToPredictedPath( + const auto ego_predicted_path = convertToPredictedPath( path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution, - prepare_duration, acceleration); + prepare_duration, prepare_acc, lane_changing_acc); const auto & vehicle_info = common_parameter.vehicle_info; auto in_lane_object_indices = dynamic_objects_indices.target_lane; @@ -454,25 +457,6 @@ bool isObjectIndexIncluded( return std::count(dynamic_objects_indices.begin(), dynamic_objects_indices.end(), index) != 0; } -double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, const double lateral_acc, - const double lateral_jerk) -{ - const auto required_time = - PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); - const auto lane_changing_length = lane_changing_velocity * required_time; - - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("calcLaneChangingLength"), - "required_time: %f [s] lane_changing_velocity : %f [m/s], lane_changing_length: %f [m]", - required_time, lane_changing_velocity, lane_changing_length); - - return lane_changing_length; -} - PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, @@ -1024,4 +1008,44 @@ boost::optional getLaneChangeTargetLane( return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } + +PredictedPath convertToPredictedPath( + const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, + const size_t nearest_seg_idx, const double duration, const double resolution, + const double prepare_time, const double prepare_acc, const double lane_changing_acc) +{ + PredictedPath predicted_path{}; + predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); + predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); + + if (path.points.empty()) { + return predicted_path; + } + + FrenetPoint vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); + const double initial_velocity = std::abs(vehicle_twist.linear.x); + + // prepare segment + for (double t = 0.0; t < prepare_time; t += resolution) { + const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + predicted_path.path.push_back( + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); + } + + // lane changing segment + const double lane_changing_velocity = + std::max(initial_velocity + prepare_acc * prepare_time, 0.0); + const double offset = + initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { + const double delta_t = t - prepare_time; + const double length = + lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; + predicted_path.path.push_back( + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); + } + + return predicted_path; +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 7a9e7ceb5a125..c1058969434d9 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -287,9 +287,9 @@ std::pair, std::vector> PathShifter::getBaseLengthsW const auto & T = total_time; const double t = T / 4; - const double s1 = v0 * t + 0.5 * a * t * t; + const double s1 = std::min(v0 * t + 0.5 * a * t * t, s); const double v1 = v0 + a * t; - const double s2 = s1 + 2 * v1 * t + 2 * a * t * t; + const double s2 = std::min(s1 + 2 * v1 * t + 2 * a * t * t, s); std::vector base_lon = {0.0, s1, s2, s}; std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; @@ -345,22 +345,22 @@ std::pair, std::vector> PathShifter::calcBaseLengths const auto ta2_tj = ta * ta * tj; const auto ta_tj2 = ta * tj * tj; - const auto s1 = tj * v0 + 0.5 * a * tj * tj; + const auto s1 = std::min(tj * v0 + 0.5 * a * tj * tj, S); const auto v1 = v0 + a * tj; - const auto s2 = s1 + ta * v1 + 0.5 * a * ta * ta; + const auto s2 = std::min(s1 + ta * v1 + 0.5 * a * ta * ta, S); const auto v2 = v1 + a * ta; - const auto s3 = s2 + tj * v2 + 0.5 * a * tj * tj; // = s4 + const auto s3 = std::min(s2 + tj * v2 + 0.5 * a * tj * tj, S); // = s4 const auto v3 = v2 + a * tj; - const auto s5 = s3 + tj * v3 + 0.5 * a * tj * tj; + const auto s5 = std::min(s3 + tj * v3 + 0.5 * a * tj * tj, S); const auto v5 = v3 + a * tj; - const auto s6 = s5 + ta * v5 + 0.5 * a * ta * ta; + const auto s6 = std::min(s5 + ta * v5 + 0.5 * a * ta * ta, S); const auto v6 = v5 + a * ta; - const auto s7 = s6 + tj * v6 + 0.5 * a * tj * tj; + const auto s7 = std::min(s6 + tj * v6 + 0.5 * a * tj * tj, S); const auto sign = shift_length > 0.0 ? 1.0 : -1.0; const auto l1 = sign * (1.0 / 6.0 * lat_jerk * tj3); diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 2de726ac82315..10e393a6daa3a 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -518,43 +518,6 @@ double l2Norm(const Vector3 vector) return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); } -PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const size_t nearest_seg_idx, const double duration, const double resolution, - const double prepare_time, const double acceleration) -{ - PredictedPath predicted_path{}; - predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); - predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); - - if (path.points.empty()) { - return predicted_path; - } - - FrenetPoint vehicle_pose_frenet = - convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); - const double initial_velocity = std::abs(vehicle_twist.linear.x); - const double lane_change_velocity = std::max(initial_velocity + acceleration * prepare_time, 0.0); - - // prepare segment - for (double t = 0.0; t < prepare_time; t += resolution) { - const double length = initial_velocity * t + 0.5 * acceleration * t * t; - predicted_path.path.push_back( - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); - } - - // lane changing segment - const double offset = - initial_velocity * prepare_time + 0.5 * acceleration * prepare_time * prepare_time; - for (double t = prepare_time; t < duration; t += resolution) { - const double length = lane_change_velocity * (t - prepare_time) + offset; - predicted_path.path.push_back( - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); - } - - return predicted_path; -} - double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, const double end_time, const double resolution)