From 05396eae10b862f96b5c5924ba85e9be8f2aee49 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Tue, 7 Nov 2023 22:09:37 +0900 Subject: [PATCH] fix(lane_change): do not cut abort path (#5509) * fix(lane_change): do not cut abort path Signed-off-by: Fumiya Watanabe * refactor(lane_change): move insert stop point Signed-off-by: Fumiya Watanabe --------- Signed-off-by: Fumiya Watanabe --- .../scene_module/lane_change/base_class.hpp | 2 +- .../scene_module/lane_change/normal.hpp | 2 +- .../scene_module/lane_change/interface.cpp | 3 +- .../src/scene_module/lane_change/normal.cpp | 80 ++++++++++--------- 4 files changed, 46 insertions(+), 41 deletions(-) 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 472564a061f04..daad5b1a40610 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 @@ -107,7 +107,7 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; - virtual bool getAbortPath() = 0; + virtual bool calcAbortPath() = 0; virtual bool specialRequiredCheck() const { return false; } 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 dc9f44fbd53da..503542be7cda6 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 @@ -72,7 +72,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo updateOutputTurnSignal() override; - bool getAbortPath() override; + bool calcAbortPath() override; PathSafetyStatus isApprovedPathSafe() const override; 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 7534c2c1fe45f..05b7c5d7dec92 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 @@ -160,7 +160,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - const auto found_abort_path = module_type_->getAbortPath(); + const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { log_warn_throttled( "Lane change path is unsafe but not found abort path. Continue lane change."); @@ -195,7 +195,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); stop_pose_ = module_type_->getStopPose(); 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 f2c6782b3ef6e..5bffef1a4b421 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 @@ -127,38 +127,43 @@ bool NormalLaneChange::isLaneChangeRequired() const LaneChangePath NormalLaneChange::getLaneChangePath() const { - return isAbortState() ? *abort_path_ : status_.lane_change_path; + return status_.lane_change_path; } BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(getLaneChangePath().path); - const auto found_extended_path = extendPath(); - if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); - } - extendOutputDrivableArea(output); - output.reference_path = std::make_shared(getReferencePath()); - output.turn_signal_info = updateOutputTurnSignal(); - - if (isAbortState()) { + if (isAbortState() && abort_path_) { + output.path = std::make_shared(abort_path_->path); output.reference_path = std::make_shared(prev_module_reference_path_); output.turn_signal_info = prev_turn_signal_info_; - return output; - } + insertStopPoint(status_.current_lanes, *output.path); + } else { + output.path = std::make_shared(getLaneChangePath().path); - if (isStopState()) { - const auto current_velocity = getEgoVelocity(); - const auto current_dist = calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); - const auto stop_dist = - -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); - setStopPose(stop_point.point.pose); + const auto found_extended_path = extendPath(); + if (found_extended_path) { + *output.path = utils::combinePath(*output.path, *found_extended_path); + } + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); + + if (isStopState()) { + const auto current_velocity = getEgoVelocity(); + const auto current_dist = calcSignedArcLength( + output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); + } else { + insertStopPoint(status_.target_lanes, *output.path); + } } + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, @@ -1500,7 +1505,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) con isAbleToStopSafely() && is_object_coming_from_rear; } -bool NormalLaneChange::getAbortPath() +bool NormalLaneChange::calcAbortPath() { const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); @@ -1608,14 +1613,12 @@ bool NormalLaneChange::getAbortPath() reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); const PathWithLaneId reference_lane_segment = std::invoke([&]() { const double s_start = arc_position.length; - double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, s_start); + double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); if (route_handler->isInGoalRouteSection(selected_path.info.target_lanes.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = - std::max(goal_arc_coordinates.length - minimum_lane_change_length, s_start); + const double forward_length = std::max(goal_arc_coordinates.length, s_start); s_end = std::min(s_end, forward_length); } PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); @@ -1625,20 +1628,23 @@ bool NormalLaneChange::getAbortPath() return ref; }); - PathWithLaneId start_to_abort_return_pose; - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); - if (reference_lane_segment.points.size() > 1) { - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), (reference_lane_segment.points.begin() + 1), - reference_lane_segment.points.end()); - } - auto abort_path = selected_path; abort_path.shifted_path = shifted_path; - abort_path.path = start_to_abort_return_pose; abort_path.info.shift_line = shift_line; + + { + PathWithLaneId aborting_path; + aborting_path.points.insert( + aborting_path.points.begin(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + + if (!reference_lane_segment.points.empty()) { + abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); + } else { + abort_path.path = aborting_path; + } + } + abort_path_ = std::make_shared(abort_path); return true; }