diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index a7a7b30e4eec0..2c3e716151df8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -135,7 +135,7 @@ AvoidancePlanningData AvoidanceByLaneChange::calcAvoidancePlanningData( const auto resample_interval = avoidance_parameters_->resample_interval_for_planning; data.reference_path = utils::resamplePathWithSpline(data.reference_path_rough, resample_interval); - data.current_lanelets = getCurrentLanes(); + data.current_lanelets = get_current_lanes(); fillAvoidanceTargetObjects(data, debug); @@ -275,7 +275,7 @@ double AvoidanceByLaneChange::calcMinAvoidanceLength(const ObjectData & nearest_ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const { - const auto current_lanes = getCurrentLanes(); + const auto current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "no empty lanes"); return std::numeric_limits::infinity(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index be62492b7c2cc..dad96d5f7371a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -51,6 +51,8 @@ class NormalLaneChange : public LaneChangeBase NormalLaneChange & operator=(NormalLaneChange &&) = delete; ~NormalLaneChange() override = default; + void update_lanes(const bool is_approved) final; + void updateLaneChangeStatus() override; std::pair getSafePath(LaneChangePath & safe_path) const override; @@ -105,8 +107,6 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo get_current_turn_signal_info() override; protected: - lanelet::ConstLanelets getCurrentLanes() const override; - lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const override; @@ -124,9 +124,7 @@ class NormalLaneChange : public LaneChangeBase const LaneChangeLanesFilteredObjects & predicted_objects, const lanelet::ConstLanelets & current_lanes) const; - LaneChangeLanesFilteredObjects filterObjects( - const lanelet::ConstLanelets & current_lanes, - const lanelet::ConstLanelets & target_lanes) const; + LaneChangeLanesFilteredObjects filterObjects() const; void filterOncomingObjects(PredictedObjects & objects) const; @@ -203,6 +201,11 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + const lanelet::ConstLanelets & get_target_lanes() const + { + return common_data_ptr_->lanes_ptr->target; + } + double stop_time_{0.0}; }; } // namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp index cbd3a81827948..c1cc14d98a7c7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/base_class.hpp @@ -65,6 +65,8 @@ class LaneChangeBase LaneChangeBase & operator=(LaneChangeBase &&) = delete; virtual ~LaneChangeBase() = default; + virtual void update_lanes(const bool is_approved) = 0; + virtual void updateLaneChangeStatus() = 0; virtual std::pair getSafePath(LaneChangePath & safe_path) const = 0; @@ -139,6 +141,11 @@ class LaneChangeBase const Twist & getEgoTwist() const { return planner_data_->self_odometry->twist.twist; } + const lanelet::ConstLanelets & get_current_lanes() const + { + return common_data_ptr_->lanes_ptr->current; + } + const BehaviorPathPlannerParameters & getCommonParam() const { return planner_data_->parameters; } LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } @@ -163,9 +170,19 @@ class LaneChangeBase common_data_ptr_->bpp_param_ptr = std::make_shared(data->parameters); } + + if (!common_data_ptr_->lanes_ptr) { + common_data_ptr_->lanes_ptr = std::make_shared(); + } + + if (!common_data_ptr_->lanes_polygon_ptr) { + common_data_ptr_->lanes_polygon_ptr = std::make_shared(); + } + common_data_ptr_->self_odometry_ptr = data->self_odometry; common_data_ptr_->route_handler_ptr = data->route_handler; common_data_ptr_->lc_param_ptr = lane_change_parameters_; + common_data_ptr_->lc_type = type_; common_data_ptr_->direction = direction_; } @@ -211,8 +228,6 @@ class LaneChangeBase virtual TurnSignalInfo get_current_turn_signal_info() = 0; protected: - virtual lanelet::ConstLanelets getCurrentLanes() const = 0; - virtual int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const = 0; virtual PathWithLaneId getPrepareSegment( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp index 2fdf7c6b550a3..1bb4dfdeb59dc 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp @@ -183,6 +183,13 @@ struct PhaseInfo } }; +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + struct Info { PhaseInfo longitudinal_acceleration{0.0, 0.0}; @@ -190,9 +197,6 @@ struct Info PhaseInfo duration{0.0, 0.0}; PhaseInfo length{0.0, 0.0}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - Pose lane_changing_start{}; Pose lane_changing_end{}; @@ -225,23 +229,26 @@ struct LanesPolygon { std::optional current; std::optional target; - std::vector target_backward; + std::optional expanded_target; + lanelet::BasicPolygon2d target_neighbor; + std::vector preceding_target; }; -struct Lanes -{ - lanelet::ConstLanelets current; - lanelet::ConstLanelets target; - std::vector preceding_target; -}; +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; +using LanesPolygonPtr = std::shared_ptr; struct CommonData { - std::shared_ptr route_handler_ptr; + RouteHandlerPtr route_handler_ptr; Odometry::ConstSharedPtr self_odometry_ptr; - std::shared_ptr bpp_param_ptr; - std::shared_ptr lc_param_ptr; - Lanes lanes; + BppParamPtr bpp_param_ptr; + LCParamPtr lc_param_ptr; + LanesPtr lanes_ptr; + LanesPolygonPtr lanes_polygon_ptr; + ModuleType lc_type; Direction direction; [[nodiscard]] Pose get_ego_pose() const { return self_odometry_ptr->pose.pose; } @@ -259,12 +266,7 @@ struct CommonData return std::hypot(x, y); } }; - -using RouteHandlerPtr = std::shared_ptr; -using BppParamPtr = std::shared_ptr; -using LCParamPtr = std::shared_ptr; using CommonDataPtr = std::shared_ptr; -using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change namespace autoware::behavior_path_planner diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp index 97b5c621deea5..5623f03a22eb3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/path.hpp @@ -23,31 +23,31 @@ #include -namespace autoware::behavior_path_planner +namespace autoware::behavior_path_planner::lane_change { using autoware::behavior_path_planner::TurnSignalInfo; using tier4_planning_msgs::msg::PathWithLaneId; -struct LaneChangePath +struct Path { PathWithLaneId path{}; ShiftedPath shifted_path{}; - LaneChangeInfo info; - bool is_safe{false}; + Info info{}; }; -using LaneChangePaths = std::vector; -struct LaneChangeStatus +struct Status { - PathWithLaneId lane_follow_path{}; - LaneChangePath lane_change_path{}; - lanelet::ConstLanelets current_lanes{}; - lanelet::ConstLanelets target_lanes{}; - std::vector lane_follow_lane_ids{}; - std::vector lane_change_lane_ids{}; + Path lane_change_path{}; bool is_safe{false}; bool is_valid_path{false}; double start_distance{0.0}; }; +} // namespace autoware::behavior_path_planner::lane_change + +namespace autoware::behavior_path_planner +{ +using LaneChangePath = lane_change::Path; +using LaneChangePaths = std::vector; +using LaneChangeStatus = lane_change::Status; } // namespace autoware::behavior_path_planner #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__PATH_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp index e25b67bdb73d8..17eaceb055fc1 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp @@ -48,6 +48,7 @@ using autoware::universe_utils::Polygon2d; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; @@ -114,8 +115,9 @@ bool isPathInLanelets( const lanelet::ConstLanelets & target_lanes); std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids); ShiftLine getLaneChangingShiftLine( @@ -177,10 +179,9 @@ bool isParkedObject( const double ratio_threshold); bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug); + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug); std::optional getLeadingStaticObjectIdx( const RouteHandler & route_handler, const LaneChangePath & lane_change_path, @@ -195,7 +196,8 @@ ExtendedPredictedObject transform( const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase); bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes); + const std::vector & collided_polygons, + const std::optional & lanes_polygon); /** * @brief Generates expanded lanelets based on the given direction and offsets. @@ -295,9 +297,11 @@ double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes); +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes); } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index b55b41828081e..4823cb0bfec22 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -75,6 +75,7 @@ void LaneChangeInterface::updateData() { universe_utils::ScopedTimeTrack st(__func__, *getTimeKeeper()); module_type_->setPreviousModuleOutput(getPreviousModuleOutput()); + module_type_->update_lanes(getCurrentStatus() == ModuleStatus::RUNNING); module_type_->updateSpecialData(); if (isWaitingApproval() || module_type_->isAbortState()) { @@ -136,7 +137,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() *prev_approved_path_ = getPreviousModuleOutput().path; BehaviorModuleOutput out = getPreviousModuleOutput(); - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().current_lanes, out.path); + module_type_->insertStopPoint(module_type_->get_current_lanes(), out.path); out.turn_signal_info = module_type_->get_current_turn_signal_info(); const auto & lane_change_debug = module_type_->getDebugData(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index 26be2af8590af..659e5b78bf577 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -41,7 +41,7 @@ namespace autoware::behavior_path_planner { using autoware::motion_utils::calcSignedArcLength; using utils::lane_change::calcMinimumLaneChangeLength; -using utils::lane_change::createLanesPolygon; +using utils::lane_change::create_lanes_polygon; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; @@ -54,6 +54,42 @@ NormalLaneChange::NormalLaneChange( stop_watch_.tic("stop_time"); } +void NormalLaneChange::update_lanes(const bool is_approved) +{ + if (is_approved) { + return; + } + + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); + + if (current_lanes.empty()) { + return; + } + + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); + if (target_lanes.empty()) { + return; + } + + const auto is_same_lanes_with_prev_iteration = + utils::lane_change::is_same_lane_with_prev_iteration( + common_data_ptr_, current_lanes, target_lanes); + + if (is_same_lanes_with_prev_iteration) { + return; + } + + common_data_ptr_->lanes_ptr->current = current_lanes; + common_data_ptr_->lanes_ptr->target = target_lanes; + + common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets( + *common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(), + common_data_ptr_->lc_param_ptr->backward_lane_length); + + *common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_); +} + void NormalLaneChange::updateLaneChangeStatus() { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -61,34 +97,23 @@ void NormalLaneChange::updateLaneChangeStatus() const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status - status_.current_lanes = status_.lane_change_path.info.current_lanes; - status_.target_lanes = status_.lane_change_path.info.target_lanes; status_.is_valid_path = found_valid_path; status_.is_safe = found_safe_path; - status_.lane_follow_lane_ids = utils::getIds(status_.current_lanes); - status_.lane_change_lane_ids = utils::getIds(status_.target_lanes); - const auto arclength_start = - lanelet::utils::getArcCoordinates(status_.target_lanes, getEgoPose()); + const auto arclength_start = lanelet::utils::getArcCoordinates(get_target_lanes(), getEgoPose()); status_.start_distance = arclength_start.length; status_.lane_change_path.path.header = getRouteHeader(); } std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) const { - const auto current_lanes = getCurrentLanes(); - - if (current_lanes.empty()) { - return {false, false}; - } + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); - const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - - if (target_lanes.empty()) { + if (current_lanes.empty() || target_lanes.empty()) { return {false, false}; } - // find candidate paths LaneChangePaths valid_paths{}; const bool is_stuck = isVehicleStuck(current_lanes); bool found_safe_path = getLaneChangePaths( @@ -104,8 +129,6 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) lane_change_debug_.valid_paths = valid_paths; if (valid_paths.empty()) { - safe_path.info.current_lanes = current_lanes; - safe_path.info.target_lanes = target_lanes; return {false, false}; } @@ -121,21 +144,22 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) bool NormalLaneChange::isLaneChangeRequired() { - status_.current_lanes = getCurrentLanes(); + const auto current_lanes = + utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); - if (status_.current_lanes.empty()) { + if (current_lanes.empty()) { return false; } - status_.target_lanes = getLaneChangeLanes(status_.current_lanes, direction_); + const auto target_lanes = getLaneChangeLanes(current_lanes, direction_); - return !status_.target_lanes.empty(); + return !target_lanes.empty(); } bool NormalLaneChange::isStoppedAtRedTrafficLight() const { return utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - status_.current_lanes, status_.lane_change_path.path, planner_data_, + get_current_lanes(), status_.lane_change_path.path, planner_data_, status_.lane_change_path.info.length.sum()); } @@ -143,7 +167,7 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() { const auto original_turn_signal_info = prev_module_output_.turn_signal_info; - const auto & current_lanes = getLaneChangeStatus().current_lanes; + const auto & current_lanes = get_current_lanes(); const auto is_valid = getLaneChangeStatus().is_valid_path; const auto & lane_change_path = getLaneChangeStatus().lane_change_path; const auto & lane_change_param = getLaneChangeParam(); @@ -236,7 +260,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const return output; } - const auto current_lanes = getCurrentLanes(); + const auto & current_lanes = get_current_lanes(); if (current_lanes.empty()) { RCLCPP_DEBUG(logger_, "Current lanes not found. Returning previous module's path as output."); return prev_module_output_; @@ -274,7 +298,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() auto output = prev_module_output_; if (isAbortState() && abort_path_) { output.path = abort_path_->path; - insertStopPoint(status_.current_lanes, output.path); + insertStopPoint(get_current_lanes(), output.path); } else { output.path = getLaneChangePath().path; @@ -294,7 +318,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path); setStopPose(stop_point.point.pose); } else { - insertStopPoint(status_.target_lanes, output.path); + insertStopPoint(get_target_lanes(), output.path); } } @@ -315,7 +339,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) c const auto & dp = planner_data_->drivable_area_expansion_parameters; const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *getRouteHandler(), status_.current_lanes, status_.target_lanes); + *getRouteHandler(), get_current_lanes(), get_target_lanes()); const auto shorten_lanes = utils::cutOverlappedLanes(output.path, drivable_lanes); const auto expanded_lanes = utils::expandLanelets( shorten_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, @@ -361,15 +385,14 @@ void NormalLaneChange::insertStopPoint( } const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - const auto target_objects = filterObjects(status_.current_lanes, status_.target_lanes); + const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; const auto is_valid_start_point = std::invoke([&]() -> bool { auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( status_.lane_change_path.info.lane_changing_start.position); - const auto target_neighbor_preferred_lane_poly_2d = - utils::lane_change::getTargetNeighborLanesPolygon( - *route_handler, status_.current_lanes, type_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; return boost::geometry::covered_by( lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); }); @@ -449,7 +472,7 @@ void NormalLaneChange::insertStopPoint( // target_objects includes objects out of target lanes, so filter them out if (!boost::geometry::intersects( autoware::universe_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - lanelet::utils::combineLaneletsShape(status_.target_lanes) + lanelet::utils::combineLaneletsShape(get_target_lanes()) .polygon2d() .basicPolygon())) { return false; @@ -476,7 +499,7 @@ PathWithLaneId NormalLaneChange::getReferencePath() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); lanelet::ConstLanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet( - status_.lane_change_path.info.target_lanes, getEgoPose(), &closest_lanelet)) { + get_target_lanes(), getEgoPose(), &closest_lanelet)) { return prev_module_output_.reference_path; } const auto reference_path = utils::getCenterLinePathFromLanelet(closest_lanelet, planner_data_); @@ -498,7 +521,7 @@ std::optional NormalLaneChange::extendPath() return std::nullopt; } - auto & target_lanes = status_.target_lanes; + auto & target_lanes = common_data_ptr_->lanes_ptr->target; const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); const auto dist_in_target = lanelet::utils::getArcCoordinates(target_lanes, getEgoPose()); @@ -569,7 +592,7 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto & pose = getEgoPose(); - const auto & current_lanes = status_.current_lanes; + const auto & current_lanes = get_current_lanes(); const auto & shift_line = status_.lane_change_path.info.shift_line; const auto & shift_path = status_.lane_change_path.shifted_path; const auto current_shift_length = lanelet::utils::getArcCoordinates(current_lanes, pose).distance; @@ -584,11 +607,6 @@ TurnSignalInfo NormalLaneChange::updateOutputTurnSignal() const return new_signal; } -lanelet::ConstLanelets NormalLaneChange::getCurrentLanes() const -{ - return utils::getCurrentLanesFromPath(prev_module_output_.path, planner_data_); -} - lanelet::ConstLanelets NormalLaneChange::getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, Direction direction) const { @@ -666,8 +684,9 @@ bool NormalLaneChange::hasFinishedLaneChange() const { const auto & current_pose = getEgoPose(); const auto & lane_change_end = status_.lane_change_path.info.shift_line.end; - const double dist_to_lane_change_end = utils::getSignedDistance( - current_pose, lane_change_end, status_.lane_change_path.info.target_lanes); + const auto & target_lanes = get_target_lanes(); + const double dist_to_lane_change_end = + utils::getSignedDistance(current_pose, lane_change_end, get_target_lanes()); double finish_judge_buffer = lane_change_parameters_->lane_change_finish_judge_buffer; // If ego velocity is low, relax finish judge buffer @@ -685,7 +704,7 @@ bool NormalLaneChange::hasFinishedLaneChange() const return false; } - const auto arc_length = lanelet::utils::getArcCoordinates(status_.target_lanes, current_pose); + const auto arc_length = lanelet::utils::getArcCoordinates(target_lanes, current_pose); const auto reach_target_lane = std::abs(arc_length.distance) < lane_change_parameters_->finish_judge_lateral_threshold; @@ -702,7 +721,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const } if (!utils::isEgoWithinOriginalLane( - status_.current_lanes, getEgoPose(), planner_data_->parameters, + get_current_lanes(), getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -723,7 +742,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; auto is_ego_within_original_lane = utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters, + get_current_lanes(), estimated_pose, planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance); lane_change_debug_.is_able_to_return_to_current_lane = is_ego_within_original_lane; return is_ego_within_original_lane; @@ -762,7 +781,7 @@ bool NormalLaneChange::isAbleToStopSafely() const if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + get_current_lanes(), estimated_pose, planner_data_->parameters); } } return true; @@ -962,8 +981,7 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects( return target_objects; } -LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes) const +LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const { const auto & current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); @@ -982,6 +1000,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( return {}; } + const auto & current_lanes = get_current_lanes(); + + if (current_lanes.empty()) { + return {}; + } + filterAheadTerminalObjects(objects, current_lanes); if (objects.objects.empty()) { @@ -992,6 +1016,12 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects( std::vector current_lane_objects; std::vector other_lane_objects; + const auto & target_lanes = get_target_lanes(); + + if (target_lanes.empty()) { + return {}; + } + filterObjectsByLanelets( objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects, other_lane_objects); @@ -1132,9 +1162,7 @@ void NormalLaneChange::filterObjectsByLanelets( }; // get backward lanes - const auto backward_length = lane_change_parameters_->backward_lane_length; - const auto target_backward_lanes = - utils::getPrecedingLanelets(*route_handler, target_lanes, current_pose, backward_length); + const auto & target_backward_lanes = common_data_ptr_->lanes_ptr->preceding_target; { lane_change_debug_.current_lanes = current_lanes; @@ -1151,12 +1179,7 @@ void NormalLaneChange::filterObjectsByLanelets( }); } - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - target_lanes, direction_, lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); - - const auto lanes_polygon = - createLanesPolygon(current_lanes, expanded_target_lanes, target_backward_lanes); + const auto & lanes_polygon = *common_data_ptr_->lanes_polygon_ptr; const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); @@ -1176,7 +1199,7 @@ void NormalLaneChange::filterObjectsByLanelets( return std::abs(lateral) > (common_parameters.vehicle_width / 2); }); - if (check_optional_polygon(object, lanes_polygon.target) && is_lateral_far) { + if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) { target_lane_objects.push_back(object); continue; } @@ -1186,7 +1209,7 @@ void NormalLaneChange::filterObjectsByLanelets( return isPolygonOverlapLanelet(object, target_backward_polygon); }; return std::any_of( - lanes_polygon.target_backward.begin(), lanes_polygon.target_backward.end(), + lanes_polygon.preceding_target.begin(), lanes_polygon.preceding_target.end(), check_backward_polygon); }); @@ -1376,14 +1399,14 @@ bool NormalLaneChange::getLaneChangePaths( 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_); + const auto & target_neighbor_preferred_lane_poly_2d = + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return false; } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes); @@ -1513,9 +1536,7 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment.points.back().point.pose.position.x, prepare_segment.points.back().point.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 target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1528,8 +1549,6 @@ bool NormalLaneChange::getLaneChangePaths( lane_change_info.velocity = LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.lateral_acceleration = lateral_acc; @@ -1558,8 +1577,8 @@ bool NormalLaneChange::getLaneChangePaths( prepare_segment, target_segment, target_lane_reference_path, shift_length); const auto candidate_path = utils::lane_change::constructCandidatePath( - lane_change_info, prepare_segment, target_segment, target_lane_reference_path, - sorted_lane_ids); + common_data_ptr_, lane_change_info, prepare_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); if (!candidate_path) { debug_print_lat("Reject: failed to generate candidate path!!"); @@ -1601,7 +1620,7 @@ bool NormalLaneChange::getLaneChangePaths( } if (utils::traffic_light::isStoppedAtRedTrafficLightWithinDistance( - lane_change_info.current_lanes, candidate_path.value().path, planner_data_, + get_current_lanes(), candidate_path.value().path, planner_data_, lane_change_info.length.sum())) { debug_print_lat("Ego is stopping near traffic light. Do not allow lane change"); continue; @@ -1609,10 +1628,10 @@ bool NormalLaneChange::getLaneChangePaths( candidate_paths->push_back(*candidate_path); if ( - !is_stuck && utils::lane_change::passParkedObject( - route_handler, *candidate_path, filtered_objects.target_lane, - lane_change_buffer, is_goal_in_route, *lane_change_parameters_, - lane_change_debug_.collision_check_objects)) { + !is_stuck && + utils::lane_change::passParkedObject( + common_data_ptr_, *candidate_path, filtered_objects.target_lane, lane_change_buffer, + is_goal_in_route, lane_change_debug_.collision_check_objects)) { debug_print_lat( "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip " "lane change."); @@ -1670,7 +1689,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( 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_); + common_data_ptr_->lanes_polygon_ptr->target_neighbor; if (target_neighbor_preferred_lane_poly_2d.empty()) { RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected."); return {}; @@ -1725,9 +1744,7 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( 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 & target_lane_poly_2d = common_data_ptr_->lanes_polygon_ptr->target.value(); const auto is_valid_start_point = boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || @@ -1739,8 +1756,6 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( 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; @@ -1780,8 +1795,8 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( 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); + common_data_ptr_, lane_change_info, reference_segment, target_segment, + target_lane_reference_path, sorted_lane_ids); return terminal_lane_change_path; } @@ -1789,10 +1804,14 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const { const auto & path = status_.lane_change_path; - const auto & current_lanes = status_.current_lanes; - const auto & target_lanes = status_.target_lanes; + const auto & current_lanes = get_current_lanes(); + const auto & target_lanes = get_target_lanes(); + + if (current_lanes.empty() || target_lanes.empty()) { + return {true, true}; + } - const auto filtered_objects = filterObjects(current_lanes, target_lanes); + const auto filtered_objects = filterObjects(); const auto target_objects = getTargetObjects(filtered_objects, current_lanes); CollisionCheckDebugMap debug_data; @@ -1847,8 +1866,8 @@ bool NormalLaneChange::isValidPath(const PathWithLaneId & path) const // check lane departure const auto drivable_lanes = utils::lane_change::generateDrivableLanes( - *route_handler, utils::extendLanes(route_handler, status_.current_lanes), - utils::extendLanes(route_handler, status_.target_lanes)); + *route_handler, utils::extendLanes(route_handler, get_current_lanes()), + utils::extendLanes(route_handler, get_target_lanes())); const auto expanded_lanes = utils::expandLanelets( drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, dp.drivable_area_types_to_skip); @@ -1882,7 +1901,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const auto threshold = lane_change_parameters_->backward_length_buffer_for_end_of_lane; if ( - isNearEndOfCurrentLanes(status_.current_lanes, status_.target_lanes, threshold) && + isNearEndOfCurrentLanes(get_current_lanes(), get_target_lanes(), threshold) && isAbleToStopSafely() && is_object_coming_from_rear) { current_lane_change_state_ = LaneChangeStates::Stop; return true; @@ -1900,16 +1919,16 @@ bool NormalLaneChange::calcAbortPath() std::max(lane_change_parameters_->minimum_lane_changing_velocity, getEgoVelocity()); const auto current_pose = getEgoPose(); const auto & selected_path = status_.lane_change_path; - const auto reference_lanelets = selected_path.info.current_lanes; const auto ego_nearest_dist_threshold = common_param.ego_nearest_dist_threshold; const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + route_handler, get_current_lanes().back(), *lane_change_parameters_, direction); const auto & lane_changing_path = selected_path.path; + const auto & reference_lanelets = get_current_lanes(); const auto lane_changing_end_pose_idx = std::invoke([&]() { constexpr double s_start = 0.0; const double s_end = std::max( @@ -1998,7 +2017,7 @@ bool NormalLaneChange::calcAbortPath() auto reference_lane_segment = prev_module_output_.path; { // const auto terminal_path = - // calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes); + // calcTerminalLaneChangePath(reference_lanelets, get_target_lanes()); // if (terminal_path) { // reference_lane_segment = terminal_path->path; // } @@ -2063,12 +2082,8 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( const auto debug_predicted_path = utils::path_safety_checker::convertToPredictedPath(ego_predicted_path, time_resolution); - const auto current_lanes = getCurrentLanes(); - - const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( - lane_change_path.info.target_lanes, direction_, - lane_change_parameters_->lane_expansion_left_offset, - lane_change_parameters_->lane_expansion_right_offset); + const auto & current_lanes_polygon = common_data_ptr_->lanes_polygon_ptr->current; + const auto & expanded_target_polygon = common_data_ptr_->lanes_polygon_ptr->expanded_target; constexpr double collision_check_yaw_diff_threshold{M_PI}; @@ -2089,10 +2104,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( continue; } - const auto collision_in_current_lanes = utils::lane_change::isCollidedPolygonsInLanelet( - collided_polygons, lane_change_path.info.current_lanes); + const auto collision_in_current_lanes = + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, current_lanes_polygon); const auto collision_in_target_lanes = - utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_lanes); + utils::lane_change::isCollidedPolygonsInLanelet(collided_polygons, expanded_target_polygon); if (!collision_in_current_lanes && !collision_in_target_lanes) { utils::path_safety_checker::updateCollisionCheckDebugMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index 9568f803773c4..20610380a0a5b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -330,13 +330,12 @@ bool isPathInLanelets( } std::optional constructCandidatePath( - const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, - const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, + const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, + const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, + const PathWithLaneId & target_lane_reference_path, const std::vector> & sorted_lane_ids) { const auto & shift_line = lane_change_info.shift_line; - const auto & original_lanes = lane_change_info.current_lanes; - const auto & target_lanes = lane_change_info.target_lanes; const auto terminal_lane_changing_velocity = lane_change_info.terminal_lane_changing_velocity; const auto longitudinal_acceleration = lane_change_info.longitudinal_acceleration; const auto lane_change_velocity = lane_change_info.velocity; @@ -393,9 +392,11 @@ std::optional constructCandidatePath( const bool enable_path_check_in_lanelet = false; // check candidate path is in lanelet + const auto & current_lanes = common_data_ptr->lanes_ptr->current; + const auto & target_lanes = common_data_ptr->lanes_ptr->target; if ( enable_path_check_in_lanelet && - !isPathInLanelets(shifted_path.path, original_lanes, target_lanes)) { + !isPathInLanelets(shifted_path.path, current_lanes, target_lanes)) { return std::nullopt; } @@ -968,17 +969,18 @@ bool isParkedObject( } bool passParkedObject( - const RouteHandler & route_handler, const LaneChangePath & lane_change_path, + const CommonDataPtr & common_data_ptr, const LaneChangePath & lane_change_path, const std::vector & objects, const double minimum_lane_change_length, - const bool is_goal_in_route, const LaneChangeParameters & lane_change_parameters, - CollisionCheckDebugMap & object_debug) + const bool is_goal_in_route, CollisionCheckDebugMap & object_debug) { + const auto route_handler = *common_data_ptr->route_handler_ptr; + const auto lane_change_parameters = *common_data_ptr->lc_param_ptr; const auto & object_check_min_road_shoulder_width = lane_change_parameters.object_check_min_road_shoulder_width; const auto & object_shiftable_ratio_threshold = lane_change_parameters.object_shiftable_ratio_threshold; const auto & path = lane_change_path.path; - const auto & current_lanes = lane_change_path.info.current_lanes; + const auto & current_lanes = common_data_ptr->lanes_ptr->current; const auto current_lane_path = route_handler.getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); @@ -1135,10 +1137,9 @@ ExtendedPredictedObject transform( } bool isCollidedPolygonsInLanelet( - const std::vector & collided_polygons, const lanelet::ConstLanelets & lanes) + const std::vector & collided_polygons, + const std::optional & lanes_polygon) { - const auto lanes_polygon = createPolygon(lanes, 0.0, std::numeric_limits::max()); - const auto is_in_lanes = [&](const auto & collided_polygon) { return lanes_polygon && boost::geometry::intersects(lanes_polygon.value(), collided_polygon); }; @@ -1207,27 +1208,60 @@ double calcPhaseLength( return std::min(length_with_acceleration, length_with_max_velocity); } -LanesPolygon createLanesPolygon( - const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, - const std::vector & target_backward_lanes) +LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr) { + const auto & lanes = common_data_ptr->lanes_ptr; LanesPolygon lanes_polygon; lanes_polygon.current = - utils::lane_change::createPolygon(current_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->current, 0.0, std::numeric_limits::max()); lanes_polygon.target = - utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits::max()); + utils::lane_change::createPolygon(lanes->target, 0.0, std::numeric_limits::max()); + + const auto & lc_param_ptr = common_data_ptr->lc_param_ptr; + const auto expanded_target_lanes = utils::lane_change::generateExpandedLanelets( + lanes->target, common_data_ptr->direction, lc_param_ptr->lane_expansion_left_offset, + lc_param_ptr->lane_expansion_right_offset); + lanes_polygon.expanded_target = utils::lane_change::createPolygon( + expanded_target_lanes, 0.0, std::numeric_limits::max()); + + const auto & route_handler = *common_data_ptr->route_handler_ptr; + lanes_polygon.target_neighbor = + getTargetNeighborLanesPolygon(route_handler, lanes->current, common_data_ptr->lc_type); - for (const auto & target_backward_lane : target_backward_lanes) { - auto lane_polygon = utils::lane_change::createPolygon( - target_backward_lane, 0.0, std::numeric_limits::max()); + lanes_polygon.preceding_target.reserve(lanes->preceding_target.size()); + for (const auto & preceding_lane : lanes->preceding_target) { + auto lane_polygon = + utils::lane_change::createPolygon(preceding_lane, 0.0, std::numeric_limits::max()); if (lane_polygon) { - lanes_polygon.target_backward.push_back(*lane_polygon); + lanes_polygon.preceding_target.push_back(*lane_polygon); } } return lanes_polygon; } + +bool is_same_lane_with_prev_iteration( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes, + const lanelet::ConstLanelets & target_lanes) +{ + if (current_lanes.empty() || target_lanes.empty()) { + return false; + } + const auto & prev_current_lanes = common_data_ptr->lanes_ptr->current; + const auto & prev_target_lanes = common_data_ptr->lanes_ptr->target; + if (prev_current_lanes.empty() || prev_target_lanes.empty()) { + return false; + } + + if ( + (prev_current_lanes.front().id() != current_lanes.front().id()) || + (prev_current_lanes.back().id() != prev_current_lanes.back().id())) { + return false; + } + return (prev_target_lanes.front().id() == target_lanes.front().id()) && + (prev_target_lanes.back().id() == prev_target_lanes.back().id()); +} } // namespace autoware::behavior_path_planner::utils::lane_change namespace autoware::behavior_path_planner::utils::lane_change::debug