From d580fe22e0a4bbe7c8bdceae9c2c24c751ae5447 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 29 Mar 2023 19:49:41 +0900 Subject: [PATCH] fix(behavior_path_planner): fix lane change speed (#3091) --- .../util/lane_change/util.hpp | 15 ++- .../util/path_shifter/path_shifter.hpp | 3 +- .../lane_change/lane_change_module.cpp | 12 +-- .../src/util/lane_change/util.cpp | 98 +++++++++++-------- .../src/util/path_shifter/path_shifter.cpp | 3 +- 5 files changed, 71 insertions(+), 60 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index bc4dfb55abfa2..d8def1bf870c3 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -54,10 +54,9 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); -std::pair calcLaneChangingSpeedAndDistance( - const double velocity, const double shift_length, const double deceleration, - const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, - const LaneChangeParameters & lc_param); +double calcLaneChangingDistance( + const double lane_changing_velocity, const double shift_length, const double min_total_lc_len, + const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, @@ -113,8 +112,8 @@ ShiftLine getLaneChangingShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_distance, const double min_total_lane_changing_distance, - const double forward_path_length, const double resample_interval, const bool is_goal_in_route); + const double lane_changing_distance, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route); PathWithLaneId getPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, @@ -128,8 +127,8 @@ PathWithLaneId getPrepareSegment( PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const double forward_path_length, const double arc_length_from_target, - const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double forward_path_length, const Pose & lane_changing_start_pose, + const double target_lane_length, const double lane_changing_distance, const double lane_changing_speed, const double total_required_min_dist); bool isEgoWithinOriginalLane( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp index ee41a91cd500a..a6c8ae67cecf0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/path_shifter/path_shifter.hpp @@ -134,8 +134,7 @@ class PathShifter static double calcLongitudinalDistFromJerk( const double lateral, const double jerk, const double velocity); - static double calcShiftTimeFromJerkAndJerk( - const double lateral, const double jerk, const double acc); + static double calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc); static double calcJerkFromLatLonDistance( const double lateral, const double longitudinal, const double velocity); 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 b5f401156271f..190178d6d5bd8 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 @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -640,12 +641,11 @@ bool LaneChangeModule::isAbortState() const bool LaneChangeModule::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); - const auto arclength_current = - lanelet::utils::getArcCoordinates(status_.lane_change_lanes, current_pose); - const double travel_distance = arclength_current.length - status_.start_distance; - const double finish_distance = - status_.lane_change_path.length.sum() + parameters_->lane_change_finish_judge_buffer; - return travel_distance > finish_distance; + const auto & lane_change_path = status_.lane_change_path.path; + const auto & lane_change_end = status_.lane_change_path.shift_line.end; + const double dist_to_lane_change_end = motion_utils::calcSignedArcLength( + lane_change_path.points, current_pose.position, lane_change_end.position); + return dist_to_lane_change_end + parameters_->lane_change_finish_judge_buffer < 0.0; } void LaneChangeModule::setObjectDebugVisualization() const diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 1f24a61cee7c5..c3b74ed68730d 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -329,7 +330,7 @@ std::pair getLaneChangePaths( const auto sorted_lane_ids = getSortedLaneIds( route_handler, original_lanelets, target_lanelets, arc_position_from_target.distance); - constexpr auto ignore_unknown{true}; + constexpr auto ignore_unknown{false}; const auto lateral_buffer = calcLateralBufferForFiltering(common_parameter.vehicle_width, 0.5); LaneChangeTargetObjectIndices dynamic_object_indices; @@ -363,35 +364,52 @@ std::pair getLaneChangePaths( std::max(prepare_speed, minimum_lane_change_velocity)); #endif - const auto estimated_shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( - target_lanelets, prepare_segment.points.front().point.pose); + if (prepare_segment.points.empty()) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), + "prepare segment is empty!! something wrong..."); + continue; + } - const auto [lane_changing_speed, lane_changing_distance] = calcLaneChangingSpeedAndDistance( - prepare_speed, estimated_shift_length, acceleration, end_of_lane_dist, common_parameter, - parameter); + // lane changing start pose is at the end of prepare segment + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; + const auto estimated_shift_length = + lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); + + // we assume constant speed during lane change + const auto lane_changing_speed = std::max(prepare_speed, minimum_lane_change_velocity); + const auto lane_changing_distance = calcLaneChangingDistance( + lane_changing_speed, estimated_shift_length, end_of_lane_dist, common_parameter, parameter); + + if (is_goal_in_route) { + const double s_start = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose).length; + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanelets, route_handler.getGoalPose()).length; + if (s_start + lane_changing_distance + parameter.lane_change_finish_judge_buffer > s_goal) { + continue; + } + } const auto lane_changing_segment = getLaneChangingSegment( - route_handler, target_lanelets, forward_path_length, arc_position_from_target.length, - target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance); + route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, + target_lane_length, lane_changing_distance, lane_changing_speed, required_total_min_distance); - if (prepare_segment.points.empty() || lane_changing_segment.points.empty()) { + if (lane_changing_segment.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), - "reference path is empty!! something wrong..."); + "lane changing segment is empty!! something wrong..."); continue; } - const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; - const auto resample_interval = calcLaneChangeResampleInterval(lane_changing_distance, lane_changing_speed); + const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; const auto target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, - lc_dist.lane_changing, required_total_min_distance, forward_path_length, resample_interval, - is_goal_in_route); + lc_dist.lane_changing, forward_path_length, resample_interval, is_goal_in_route); if (target_lane_reference_path.points.empty()) { continue; @@ -482,9 +500,7 @@ bool hasEnoughDistance( const double lane_change_required_distance = static_cast(num) * minimum_lane_change_length; - if ( - lane_change_total_distance + lane_change_required_distance > - util::getDistanceToEndOfLane(current_pose, current_lanes)) { + if (lane_change_total_distance > util::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } @@ -710,17 +726,14 @@ PathWithLaneId getPrepareSegment( return prepare_segment; } -std::pair calcLaneChangingSpeedAndDistance( - const double velocity, const double shift_length, const double deceleration, - const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, - const LaneChangeParameters & lc_param) +double calcLaneChangingDistance( + const double lane_changing_speed, const double shift_length, const double min_total_lc_len, + const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) { - const auto required_time = PathShifter::calcShiftTimeFromJerkAndJerk( + const auto required_time = PathShifter::calcShiftTimeFromJerk( shift_length, lc_param.lane_changing_lateral_jerk, lc_param.lane_changing_lateral_acc); - const auto lane_changing_average_speed = - std::max(velocity + deceleration * 0.5 * required_time, lc_param.minimum_lane_change_velocity); - const auto expected_dist = lane_changing_average_speed * required_time; + const auto expected_dist = lane_changing_speed * required_time; const auto lane_changing_distance = (expected_dist < min_total_lc_len) ? expected_dist : com_param.minimum_lane_change_length; @@ -730,22 +743,25 @@ std::pair calcLaneChangingSpeedAndDistance( .get_child("util") .get_child("calcLaneChangingSpeedAndDistance"), "required_time: %f [s] average_speed: %f [m/s], lane_changing_distance : %f [m]", required_time, - lane_changing_average_speed, lane_changing_distance); + lane_changing_speed, lane_changing_distance); - return {lane_changing_average_speed, lane_changing_distance}; + return lane_changing_distance; } PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, - const double forward_path_length, const double arc_length_from_target, - const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, + const double forward_path_length, const Pose & lane_changing_start_pose, + const double target_lane_length, const double lane_changing_distance, const double lane_changing_speed, const double total_required_min_dist) { - const double s_start = std::invoke([&arc_length_from_target, &dist_prepare_to_lc_end, - &target_lane_length, &total_required_min_dist]() { - const double dist_from_current_pose = arc_length_from_target + dist_prepare_to_lc_end.sum(); + const double s_start = std::invoke([&lane_changing_start_pose, &target_lanelets, + &lane_changing_distance, &target_lane_length, + &total_required_min_dist]() { + const auto arc_to_start_pose = + lanelet::utils::getArcCoordinates(target_lanelets, lane_changing_start_pose); + const double dist_from_front_target_lanelet = arc_to_start_pose.length + lane_changing_distance; const double end_of_lane_dist_without_buffer = target_lane_length - total_required_min_dist; - return std::min(dist_from_current_pose, end_of_lane_dist_without_buffer); + return std::min(dist_from_front_target_lanelet, end_of_lane_dist_without_buffer); }); const double s_end = @@ -776,8 +792,8 @@ PathWithLaneId getLaneChangingSegment( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const double lane_changing_distance, const double min_total_lane_changing_distance, - const double forward_path_length, const double resample_interval, const bool is_goal_in_route) + const double lane_changing_distance, const double forward_path_length, + const double resample_interval, const bool is_goal_in_route) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -786,13 +802,11 @@ PathWithLaneId getReferencePathFromTargetLane( const double s_end = std::invoke([&]() { const auto dist_from_lc_start = s_start + lane_changing_distance + forward_path_length; if (is_goal_in_route) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); - const auto dist_to_goal = goal_arc_coordinates.length - min_total_lane_changing_distance; - return std::min(dist_from_lc_start, dist_to_goal); + const double s_goal = + lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()).length; + return std::min(dist_from_lc_start, s_goal); } - const auto dist_from_end = target_lane_length - min_total_lane_changing_distance; - return std::min(dist_from_lc_start, dist_from_end); + return std::min(dist_from_lc_start, target_lane_length); }); RCLCPP_DEBUG( diff --git a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp index aa90563f16a33..4e0ec714031bd 100644 --- a/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/util/path_shifter/path_shifter.cpp @@ -473,8 +473,7 @@ void PathShifter::shiftBaseLength(ShiftedPath * path, double offset) const } } -double PathShifter::calcShiftTimeFromJerkAndJerk( - const double lateral, const double jerk, const double acc) +double PathShifter::calcShiftTimeFromJerk(const double lateral, const double jerk, const double acc) { const double j = std::abs(jerk); const double a = std::abs(acc);