diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 9c3c371627a78..e4af48a71c8f3 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -53,9 +53,11 @@ class NormalLaneChange : public LaneChangeBase LaneChangePath getLaneChangePath() const override; + BehaviorModuleOutput getTerminalLaneChangePath() const override; + BehaviorModuleOutput generateOutput() override; - void extendOutputDrivableArea(BehaviorModuleOutput & output) override; + void extendOutputDrivableArea(BehaviorModuleOutput & output) const override; void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override; @@ -65,7 +67,7 @@ class NormalLaneChange : public LaneChangeBase void resetParameters() override; - TurnSignalInfo updateOutputTurnSignal() override; + TurnSignalInfo updateOutputTurnSignal() const override; bool calcAbortPath() override; @@ -89,7 +91,7 @@ class NormalLaneChange : public LaneChangeBase bool isAbortState() const override; - bool isLaneChangeRequired() const override; + bool isLaneChangeRequired() override; bool isStoppedAtRedTrafficLight() const override; @@ -141,7 +143,11 @@ class NormalLaneChange : public LaneChangeBase const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety = true) const override; - TurnSignalInfo calcTurnSignalInfo() override; + std::optional calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) const; + + TurnSignalInfo calcTurnSignalInfo() const override; bool isValidPath(const PathWithLaneId & path) const override; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp index 451899fbf27e6..001af46bbcbf0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp @@ -66,7 +66,7 @@ class LaneChangeBase virtual BehaviorModuleOutput generateOutput() = 0; - virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) = 0; + virtual void extendOutputDrivableArea(BehaviorModuleOutput & output) const = 0; virtual PathWithLaneId getReferencePath() const = 0; @@ -74,13 +74,13 @@ class LaneChangeBase virtual void resetParameters() = 0; - virtual TurnSignalInfo updateOutputTurnSignal() = 0; + virtual TurnSignalInfo updateOutputTurnSignal() const = 0; virtual bool hasFinishedLaneChange() const = 0; virtual bool hasFinishedAbort() const = 0; - virtual bool isLaneChangeRequired() const = 0; + virtual bool isLaneChangeRequired() = 0; virtual bool isAbortState() const = 0; @@ -88,6 +88,8 @@ class LaneChangeBase virtual LaneChangePath getLaneChangePath() const = 0; + virtual BehaviorModuleOutput getTerminalLaneChangePath() const = 0; + virtual bool isEgoOnPreparePhase() const = 0; virtual bool isRequiredStop(const bool is_object_coming_from_rear) = 0; @@ -225,7 +227,7 @@ class LaneChangeBase LaneChangePaths * candidate_paths, const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck, const bool check_safety) const = 0; - virtual TurnSignalInfo calcTurnSignalInfo() = 0; + virtual TurnSignalInfo calcTurnSignalInfo() const = 0; virtual bool isValidPath(const PathWithLaneId & path) const = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp index 9e42b49635b82..3af4f487a0fa0 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/path.hpp @@ -45,7 +45,7 @@ struct LaneChangeStatus std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; - bool is_valid_path{true}; + bool is_valid_path{false}; double start_distance{0.0}; }; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 67506326d92aa..e16afcdbf19ec 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -115,6 +115,10 @@ ShiftLine getLaneChangingShiftLine( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & reference_path, const double shift_length); +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length); + PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 448f83ecaf15e..9d6f27f283fe4 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -50,9 +50,6 @@ LaneChangeInterface::LaneChangeInterface( void LaneChangeInterface::processOnEntry() { waitApproval(); - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); } void LaneChangeInterface::processOnExit() @@ -73,13 +70,21 @@ bool LaneChangeInterface::isExecutionRequested() const bool LaneChangeInterface::isExecutionReady() const { - return module_type_->isSafe(); + return module_type_->isSafe() && !module_type_->isAbortState(); } void LaneChangeInterface::updateData() { module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); + module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); + module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); + + if (isWaitingApproval()) { + module_type_->updateLaneChangeStatus(); + } + setObjectDebugVisualization(); + module_type_->updateSpecialData(); module_type_->resetStopPose(); } @@ -98,8 +103,6 @@ BehaviorModuleOutput LaneChangeInterface::plan() return {}; } - module_type_->setPreviousDrivableAreaInfo(getPreviousModuleOutput().drivable_area_info); - module_type_->setPreviousTurnSignalInfo(getPreviousModuleOutput().turn_signal_info); auto output = module_type_->generateOutput(); path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; @@ -112,7 +115,16 @@ BehaviorModuleOutput LaneChangeInterface::plan() } updateSteeringFactorPtr(output); - clearWaitingApproval(); + if (module_type_->isAbortState()) { + waitApproval(); + removeRTCStatus(); + const auto candidate = planCandidate(); + path_candidate_ = std::make_shared(candidate.path_candidate); + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); + } else { + clearWaitingApproval(); + } return output; } @@ -120,30 +132,19 @@ BehaviorModuleOutput LaneChangeInterface::plan() BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() { *prev_approved_path_ = getPreviousModuleOutput().path; - module_type_->insertStopPoint( - module_type_->getLaneChangeStatus().current_lanes, *prev_approved_path_); BehaviorModuleOutput out; - out.path = *prev_approved_path_; - out.reference_path = getPreviousModuleOutput().reference_path; - out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; - out.drivable_area_info = getPreviousModuleOutput().drivable_area_info; - - module_type_->setPreviousModulePaths( - getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); - module_type_->updateLaneChangeStatus(); - setObjectDebugVisualization(); + out = module_type_->getTerminalLaneChangePath(); + module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + out.turn_signal_info = + getCurrentTurnSignalInfo(out.path, getPreviousModuleOutput().turn_signal_info); for (const auto & [uuid, data] : module_type_->getDebugData()) { const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); } - // change turn signal when the vehicle reaches at the end of the path for waiting lane change - out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info); - path_reference_ = std::make_shared(getPreviousModuleOutput().reference_path); - stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { @@ -211,6 +212,7 @@ bool LaneChangeInterface::canTransitSuccessState() } if (module_type_->hasFinishedLaneChange()) { + module_type_->resetParameters(); log_debug_throttled("Lane change process has completed."); return true; } @@ -468,16 +470,13 @@ TurnSignalInfo LaneChangeInterface::getCurrentTurnSignalInfo( const double buffer = next_lane_change_buffer + min_length_for_turn_signal_activation + base_to_front; const double path_length = motion_utils::calcArcLength(path.points); - const auto & front_point = path.points.front().point.pose.position; const size_t & current_nearest_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); - const double length_front_to_ego = motion_utils::calcSignedArcLength( - path.points, front_point, static_cast(0), current_pose.position, - current_nearest_seg_idx); + const double dist_to_terminal = utils::getDistanceToEndOfLane(current_pose, current_lanes); const auto start_pose = motion_utils::calcLongitudinalOffsetPose(path.points, 0, std::max(path_length - buffer, 0.0)); - if (path_length - length_front_to_ego < buffer && start_pose) { + if (dist_to_terminal - base_to_front < buffer && start_pose) { // modify turn signal current_turn_signal_info.desired_start_point = *start_pose; current_turn_signal_info.desired_end_point = lane_change_path.info.lane_changing_end; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c7ddd8fe63636..a4587c0b97aa0 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -115,17 +115,17 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) return {true, found_safe_path}; } -bool NormalLaneChange::isLaneChangeRequired() const +bool NormalLaneChange::isLaneChangeRequired() { - const auto current_lanes = getCurrentLanes(); + status_.current_lanes = getCurrentLanes(); - if (current_lanes.empty()) { + if (status_.current_lanes.empty()) { return false; } - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); - return !target_lanes.empty(); + return !status_.target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const @@ -140,6 +140,59 @@ LaneChangePath NormalLaneChange::getLaneChangePath() const return status_.lane_change_path; } +BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const +{ + BehaviorModuleOutput output; + + if (isAbortState() && abort_path_) { + output.path = abort_path_->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = prev_turn_signal_info_; + 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, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + return output; + } + + const auto current_lanes = getCurrentLanes(); + if (current_lanes.empty()) { + RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong."); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + const auto terminal_path = + calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_)); + if (!terminal_path) { + RCLCPP_DEBUG(logger_, "Terminal path not found!!!"); + output.path = prev_module_path_; + output.reference_path = prev_module_reference_path_; + output.drivable_area_info = prev_drivable_area_info_; + output.turn_signal_info = prev_turn_signal_info_; + return output; + } + + output.path = terminal_path->path; + output.reference_path = prev_module_reference_path_; + output.turn_signal_info = updateOutputTurnSignal(); + + 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, + planner_data_->parameters.ego_nearest_dist_threshold, + planner_data_->parameters.ego_nearest_yaw_threshold); + + return output; +} + BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; @@ -183,7 +236,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() return output; } -void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) +void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) const { const auto & dp = planner_data_->drivable_area_expansion_parameters; @@ -420,11 +473,18 @@ void NormalLaneChange::resetParameters() is_abort_approval_requested_ = false; current_lane_change_state_ = LaneChangeStates::Normal; abort_path_ = nullptr; + status_ = {}; object_debug_.clear(); + object_debug_after_approval_.clear(); + debug_filtered_objects_.current_lane.clear(); + debug_filtered_objects_.target_lane.clear(); + debug_filtered_objects_.other_lane.clear(); + debug_valid_path_.clear(); + RCLCPP_DEBUG(logger_, "reset all flags and debug information."); } -TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() +TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { TurnSignalInfo turn_signal_info = calcTurnSignalInfo(); const auto [turn_signal_command, distance_to_vehicle_front] = utils::getPathTurnSignal( @@ -833,6 +893,10 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( const auto current_pose = getEgoPose(); const auto & route_handler = *getRouteHandler(); const auto & common_parameters = planner_data_->parameters; + const auto shift_intervals = + route_handler.getLateralIntervalsToPreferredLane(current_lanes.back(), direction_); + const double minimum_lane_change_length = + utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); // Guard if (objects.objects.empty()) { @@ -902,6 +966,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }; + // ignore object that are ahead of terminal lane change start + { + double distance_to_terminal_from_object = std::numeric_limits::max(); + for (const auto & polygon_p : obj_polygon_outer) { + const auto obj_p = tier4_autoware_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0); + Pose polygon_pose; + polygon_pose.position = obj_p; + polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation; + const double dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes); + if (dist < distance_to_terminal_from_object) { + distance_to_terminal_from_object = dist; + } + } + if (minimum_lane_change_length > distance_to_terminal_from_object) { + continue; + } + } + // ignore static object that are behind the ego vehicle if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) { continue; @@ -1409,6 +1491,151 @@ bool NormalLaneChange::getLaneChangePaths( return false; } +std::optional NormalLaneChange::calcTerminalLaneChangePath( + const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +{ + if (current_lanes.empty() || target_lanes.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + const auto & route_handler = *getRouteHandler(); + const auto & common_parameters = planner_data_->parameters; + + const auto forward_path_length = common_parameters.forward_path_length; + const auto minimum_lane_changing_velocity = + lane_change_parameters_->minimum_lane_changing_velocity; + + const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); + + 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 target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); + + const auto sorted_lane_ids = + utils::lane_change::getSortedLaneIds(route_handler, getEgoPose(), current_lanes, target_lanes); + + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon(route_handler, current_lanes, type_); + if (target_neighbor_preferred_lane_poly_2d.empty()) { + RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); + return {}; + } + + // lane changing start getEgoPose() is at the end of prepare segment + const auto current_lane_terminal_point = + lanelet::utils::conversion::toGeomMsgPt(current_lanes.back().centerline3d().back()); + + double distance_to_terminal_from_goal = 0; + if (is_goal_in_route) { + distance_to_terminal_from_goal = + utils::getDistanceToEndOfLane(route_handler.getGoalPose(), current_lanes); + } + + const auto lane_changing_start_pose = motion_utils::calcLongitudinalOffsetPose( + prev_module_path_.points, current_lane_terminal_point, + -(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal)); + + if (!lane_changing_start_pose) { + RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!"); + return {}; + } + + const auto target_length_from_lane_change_start_pose = utils::getArcLengthToTargetLanelet( + current_lanes, target_lanes.front(), lane_changing_start_pose.value()); + + // Check if the lane changing start point is not on the lanes next to target lanes, + if (target_length_from_lane_change_start_pose > 0.0) { + RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!"); + return {}; + } + + const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet( + target_lanes, lane_changing_start_pose.value()); + + const auto [min_lateral_acc, max_lateral_acc] = + lane_change_parameters_->lane_change_lat_acc_map.find(minimum_lane_changing_velocity); + + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, lane_change_parameters_->lane_changing_lateral_jerk, max_lateral_acc); + + const auto target_segment = getTargetSegment( + target_lanes, lane_changing_start_pose.value(), target_lane_length, lane_change_buffer, + minimum_lane_changing_velocity, next_lane_change_buffer); + + if (target_segment.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong..."); + return {}; + } + + const lanelet::BasicPoint2d lc_start_point( + lane_changing_start_pose->position.x, lane_changing_start_pose->position.y); + + const auto target_lane_polygon = + lanelet::utils::getPolygonFromArcLength(target_lanes, 0, std::numeric_limits::max()); + const auto target_lane_poly_2d = lanelet::utils::to2D(target_lane_polygon).basicPolygon(); + + const auto is_valid_start_point = + boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || + boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = LaneChangePhaseInfo{0.0, 0.0}; + lane_change_info.duration = LaneChangePhaseInfo{0.0, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{minimum_lane_changing_velocity, minimum_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{0.0, lane_change_buffer}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = lane_changing_start_pose.value(); + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = max_lateral_acc; + lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity; + + if (!is_valid_start_point) { + RCLCPP_DEBUG( + logger_, + "Reject: lane changing points are not inside of the target preferred lanes or its " + "neighbors"); + return {}; + } + + const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( + lane_change_buffer, minimum_lane_changing_velocity); + const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( + route_handler, target_lanes, lane_changing_start_pose.value(), target_lane_length, + lane_change_buffer, forward_path_length, resample_interval, is_goal_in_route, + next_lane_change_buffer); + + if (target_lane_reference_path.points.empty()) { + RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!"); + return {}; + } + + lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( + lane_changing_start_pose.value(), target_segment.points.front().point.pose, + target_lane_reference_path, shift_length); + + auto reference_segment = prev_module_path_; + const double length_to_lane_changing_start = motion_utils::calcSignedArcLength( + reference_segment.points, reference_segment.points.front().point.pose.position, + lane_changing_start_pose->position); + utils::clipPathLength(reference_segment, 0, length_to_lane_changing_start, 0.0); + // remove terminal points because utils::clipPathLength() calculates extra long path + reference_segment.points.pop_back(); + reference_segment.points.back().point.longitudinal_velocity_mps = minimum_lane_changing_velocity; + + const auto terminal_lane_change_path = utils::lane_change::constructCandidatePath( + lane_change_info, reference_segment, target_segment, target_lane_reference_path, + sorted_lane_ids); + + return terminal_lane_change_path; +} + PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; @@ -1440,7 +1667,7 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const return safety_status; } -TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() +TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() const { const auto get_blinker_pose = [](const PathWithLaneId & path, const double length) { double accumulated_length = 0.0; @@ -1460,6 +1687,10 @@ TurnSignalInfo NormalLaneChange::calcTurnSignalInfo() TurnSignalInfo turn_signal_info{}; + if (path.path.points.empty()) { + return turn_signal_info; + } + // desired start pose = prepare start pose turn_signal_info.desired_start_point = std::invoke([&]() { const auto blinker_start_duration = planner_data_->parameters.turn_signal_search_time; @@ -1639,31 +1870,25 @@ bool NormalLaneChange::calcAbortPath() ShiftedPath shifted_path; // offset front side - // bool offset_back = false; if (!path_shifter.generate(&shifted_path)) { RCLCPP_ERROR(logger_, "failed to generate abort shifted path."); } - const auto arc_position = lanelet::utils::getArcCoordinates( - 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), s_start); - - if ( - !reference_lanelets.empty() && - route_handler->isInGoalRouteSection(reference_lanelets.back())) { - const auto goal_arc_coordinates = - lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = std::max(goal_arc_coordinates.length, s_start); - s_end = std::min(s_end, forward_length); + PathWithLaneId reference_lane_segment = prev_module_path_; + { + const auto terminal_path = + calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + if (terminal_path) { + reference_lane_segment = terminal_path->path; } - 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(lane_change_parameters_->minimum_lane_changing_velocity)); - return ref; - }); + const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose; + const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold, + common_param.ego_nearest_yaw_threshold); + reference_lane_segment.points = motion_utils::cropPoints( + reference_lane_segment.points, return_pose.position, seg_idx, + common_param.forward_path_length, 0.0); + } auto abort_path = selected_path; abort_path.shifted_path = shifted_path; diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 2d0bc9b762913..a331d418d5e46 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -428,10 +428,6 @@ PathWithLaneId getReferencePathFromTargetLane( return std::min(dist_from_lc_start, target_lane_length - next_lane_change_buffer); }); - if (s_end - s_start < lane_changing_length) { - return PathWithLaneId(); - } - RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") @@ -439,6 +435,10 @@ PathWithLaneId getReferencePathFromTargetLane( .get_child("getReferencePathFromTargetLane"), "start: %f, end: %f", s_start, s_end); + if (s_end - s_start < lane_changing_length) { + return PathWithLaneId(); + } + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); @@ -453,6 +453,14 @@ ShiftLine getLaneChangingShiftLine( const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; const Pose & lane_changing_end_pose = target_segment.points.front().point.pose; + return getLaneChangingShiftLine( + lane_changing_start_pose, lane_changing_end_pose, reference_path, shift_length); +} + +ShiftLine getLaneChangingShiftLine( + const Pose & lane_changing_start_pose, const Pose & lane_changing_end_pose, + const PathWithLaneId & reference_path, const double shift_length) +{ ShiftLine shift_line; shift_line.end_shift_length = shift_length; shift_line.start = lane_changing_start_pose;