From c734d1a167d0fb28a75b7bb437d9f30148b789af Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 5 Nov 2024 19:21:26 +0900 Subject: [PATCH 1/6] yield at intersection by making the check conservative Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 2 + .../utils/base_class.hpp | 2 + .../utils/data_structs.hpp | 1 + .../utils/path.hpp | 6 ++ .../utils/utils.hpp | 7 ++ .../src/interface.cpp | 9 +++ .../src/scene.cpp | 75 +++++++++++++++++++ .../src/utils/utils.cpp | 47 ++++++++++++ 8 files changed, 149 insertions(+) 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 04f21f5249240..3e8802afbb23e 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 @@ -98,6 +98,8 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; + bool is_safe_to_try_change_lanes_at_intersection() const final; + protected: lanelet::ConstLanelets getCurrentLanes() 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 9c891389c4571..0df95ec23f7f7 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 @@ -84,6 +84,8 @@ class LaneChangeBase virtual bool isAbortState() const = 0; + virtual bool is_safe_to_try_change_lanes_at_intersection() const = 0; + virtual bool isAbleToReturnCurrentLane() const = 0; virtual LaneChangePath getLaneChangePath() const = 0; diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp index 5039dea7c0db6..2903a0afbc978 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/data_structs.hpp @@ -93,6 +93,7 @@ struct LaneChangeParameters // lane change parameters double backward_length_buffer_for_end_of_lane; double backward_length_buffer_for_blocking_object; + double backward_length_from_intersection{5.0}; double lane_changing_lateral_jerk{0.5}; double minimum_lane_changing_velocity{5.6}; double lane_change_prepare_duration{4.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..fc8fbd2da01ef 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 @@ -21,6 +21,7 @@ #include +#include #include namespace behavior_path_planner @@ -39,14 +40,19 @@ using LaneChangePaths = std::vector; struct LaneChangeStatus { PathWithLaneId lane_follow_path{}; + PathWithLaneId path_after_intersection{}; LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; + lanelet::ConstLanelet current_lane_{}; std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; bool is_valid_path{true}; + bool is_ego_in_turn_direction_lane{false}; + bool is_ego_in_intersection{false}; double start_distance{0.0}; + double distance_from_prev_intersection{std::numeric_limits::max()}; }; } // namespace behavior_path_planner 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 48b940093c2cd..ee52a76657288 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 @@ -43,6 +43,7 @@ using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_perception_msgs::msg::PredictedPath; using autoware_auto_planning_msgs::msg::PathWithLaneId; using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject; +using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPolygonStamped; using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; @@ -338,5 +339,11 @@ bool has_blocking_target_object( const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects, const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, const PathWithLaneId & path); + +std::vector get_line_string_paths(const ExtendedPredictedObject & object); + +bool has_overtaking_turn_lane_object( + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane, + const ExtendedPredictedObjects & trailing_objects); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 61699a8b35bdc..11c51eea231d8 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -120,6 +120,15 @@ ModuleStatus LaneChangeInterface::updateState() module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe()); setObjectDebugVisualization(); + + if ( + !module_type_->is_safe_to_try_change_lanes_at_intersection() && + module_type_->isAbleToReturnCurrentLane()) { + RCLCPP_DEBUG( + logger_, "unsafe to try lane change at intersection but able to return to current lane"); + return ModuleStatus::SUCCESS; + } + if (is_safe) { log_warn_throttled("Lane change path is safe."); module_type_->toNormalState(); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 284ea9409e924..1abbad931cc8b 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -54,6 +54,51 @@ NormalLaneChange::NormalLaneChange( void NormalLaneChange::updateLaneChangeStatus() { updateStopTime(); + status_.current_lanes = getCurrentLanes(); + if (status_.current_lanes.empty()) { + return; + } + + lanelet::ConstLanelet current_lane; + if (!getRouteHandler()->getClosestLaneletWithinRoute(getEgoPose(), ¤t_lane)) { + RCLCPP_DEBUG(logger_, "ego's current lane not in route"); + return; + } + status_.current_lane_ = current_lane; + + const auto ego_footprint = + utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info); + status_.is_ego_in_turn_direction_lane = + utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); + status_.is_ego_in_intersection = + utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint); + + if ( + status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane && + status_.path_after_intersection.points.empty()) { + const auto target_neighbor = + utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_); + status_.path_after_intersection = getRouteHandler()->getCenterLinePath( + target_neighbor, 0.0, std::numeric_limits::max()); + status_.distance_from_prev_intersection = 0.0; + } + + if ( + !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane && + !status_.path_after_intersection.points.empty()) { + status_.distance_from_prev_intersection = calcSignedArcLength( + status_.path_after_intersection.points, + status_.path_after_intersection.points.front().point.pose.position, getEgoPosition()); + } + + if ( + !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane && + status_.distance_from_prev_intersection > + lane_change_parameters_->backward_length_from_intersection && + !status_.path_after_intersection.points.empty()) { + status_.path_after_intersection = PathWithLaneId(); + status_.distance_from_prev_intersection = std::numeric_limits::max(); + } const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -135,6 +180,24 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const status_.lane_change_path.info.length.sum()); } +bool NormalLaneChange::is_safe_to_try_change_lanes_at_intersection() const +{ + if (debug_filtered_objects_.target_lane.empty() || status_.target_lanes.empty()) { + return true; + } + + const auto has_overtaking_turn_direction_object = + utils::lane_change::has_overtaking_turn_lane_object( + status_.target_lanes, status_.current_lane_, debug_filtered_objects_.target_lane); + + if (!has_overtaking_turn_direction_object) { + return true; + } + + return status_.distance_from_prev_intersection > + lane_change_parameters_->backward_length_from_intersection; +} + LaneChangePath NormalLaneChange::getLaneChangePath() const { return status_.lane_change_path; @@ -1372,6 +1435,18 @@ bool NormalLaneChange::getLaneChangePaths( } candidate_paths->push_back(*candidate_path); + if (status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane) { + return false; + } + if ( + status_.distance_from_prev_intersection < + lane_change_parameters_->backward_length_from_intersection && + utils::lane_change::has_overtaking_turn_lane_object( + status_.target_lanes, status_.current_lane_, target_objects.target_lane)) { + RCLCPP_DEBUG(logger_, "has overtaking object while ego leaves intersection"); + return false; + } + std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( 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 0d5b20db3c0c8..ded41f3cda26c 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1305,4 +1305,51 @@ bool has_blocking_target_object( return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } + +std::vector get_line_string_paths(const ExtendedPredictedObject & object) +{ + const auto transform = [](const auto & predicted_path) -> LineString2d { + LineString2d line_string; + const auto & path = predicted_path.path; + line_string.reserve(path.size()); + for (const auto & path_point : path) { + const auto point = tier4_autoware_utils::fromMsg(path_point.pose.position).to_2d(); + line_string.push_back(point); + } + + return line_string; + }; + + const auto paths = object.predicted_paths; + std::vector line_strings; + std::transform(paths.begin(), paths.end(), std::back_inserter(line_strings), transform); + + return line_strings; +} + +bool has_overtaking_turn_lane_object( + const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane, + const ExtendedPredictedObjects & trailing_objects) +{ + const auto target_lane_poly = + lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); + const auto is_overlap_with_target = [&](const LineString2d & path) { + return !boost::geometry::disjoint(path, target_lane_poly); + }; + + return std::any_of( + trailing_objects.begin(), trailing_objects.end(), [&](const ExtendedPredictedObject & object) { + lanelet::ConstLanelet obj_lane; + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); + if (!boost::geometry::disjoint( + obj_poly, utils::toPolygon2d( + lanelet::utils::to2D(ego_current_lane.polygon2d().basicPolygon())))) { + return true; + } + + const auto paths = get_line_string_paths(object); + return std::any_of(paths.begin(), paths.end(), is_overlap_with_target); + }); +} } // namespace behavior_path_planner::utils::lane_change From f6de32d656fbe80200b245137d315a0e2468febd Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 5 Nov 2024 19:21:45 +0900 Subject: [PATCH 2/6] make code of similar to main branch Signed-off-by: Zulfaqar Azmi --- .../config/lane_change.param.yaml | 2 + .../scene.hpp | 5 +- .../utils/base_class.hpp | 2 - .../utils/path.hpp | 5 +- .../utils/utils.hpp | 5 +- .../src/interface.cpp | 8 -- .../src/manager.cpp | 2 + .../src/scene.cpp | 93 +++++++++---------- .../src/utils/utils.cpp | 22 ++++- 9 files changed, 79 insertions(+), 65 deletions(-) diff --git a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml index 1ab33514c5f24..c9dc2922b2078 100644 --- a/planning/behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,6 +6,8 @@ backward_length_buffer_for_end_of_lane: 3.0 # [m] backward_length_buffer_for_blocking_object: 3.0 # [m] + backward_length_from_intersection: 5.0 # [m] + lane_change_finish_judge_buffer: 2.0 # [m] lane_changing_lateral_jerk: 0.5 # [m/s3] 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 3e8802afbb23e..3ae70cbdae91d 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 @@ -98,8 +98,6 @@ class NormalLaneChange : public LaneChangeBase bool isStoppedAtRedTrafficLight() const override; - bool is_safe_to_try_change_lanes_at_intersection() const final; - protected: lanelet::ConstLanelets getCurrentLanes() const override; @@ -188,6 +186,9 @@ class NormalLaneChange : public LaneChangeBase double getStopTime() const { return stop_time_; } + void update_dist_from_intersection(); + + std::vector path_after_intersection_; double stop_time_{0.0}; }; } // namespace behavior_path_planner 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 0df95ec23f7f7..9c891389c4571 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 @@ -84,8 +84,6 @@ class LaneChangeBase virtual bool isAbortState() const = 0; - virtual bool is_safe_to_try_change_lanes_at_intersection() const = 0; - virtual bool isAbleToReturnCurrentLane() const = 0; virtual LaneChangePath getLaneChangePath() 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 fc8fbd2da01ef..f4fd6d4816413 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 @@ -40,11 +40,10 @@ using LaneChangePaths = std::vector; struct LaneChangeStatus { PathWithLaneId lane_follow_path{}; - PathWithLaneId path_after_intersection{}; LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; - lanelet::ConstLanelet current_lane_{}; + lanelet::ConstLanelet current_lane{}; std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; @@ -52,7 +51,7 @@ struct LaneChangeStatus bool is_ego_in_turn_direction_lane{false}; bool is_ego_in_intersection{false}; double start_distance{0.0}; - double distance_from_prev_intersection{std::numeric_limits::max()}; + double dist_from_prev_intersection{std::numeric_limits::max()}; }; } // namespace behavior_path_planner 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 ee52a76657288..f8ca377ccac65 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 @@ -340,10 +340,13 @@ bool has_blocking_target_object( const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose, const PathWithLaneId & path); +bool has_passed_intersection_turn_direction( + const LaneChangeParameters & lc_param, const LaneChangeStatus & status); + std::vector get_line_string_paths(const ExtendedPredictedObject & object); bool has_overtaking_turn_lane_object( - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane, + const LaneChangeStatus & status, const LaneChangeParameters & lc_param, const ExtendedPredictedObjects & trailing_objects); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp index 11c51eea231d8..c734e845d850c 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -121,14 +121,6 @@ ModuleStatus LaneChangeInterface::updateState() setObjectDebugVisualization(); - if ( - !module_type_->is_safe_to_try_change_lanes_at_intersection() && - module_type_->isAbleToReturnCurrentLane()) { - RCLCPP_DEBUG( - logger_, "unsafe to try lane change at intersection but able to return to current lane"); - return ModuleStatus::SUCCESS; - } - if (is_safe) { log_warn_throttled("Lane change path is safe."); module_type_->toNormalState(); diff --git a/planning/behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_lane_change_module/src/manager.cpp index a854a4984d261..3cba445642de9 100644 --- a/planning/behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_lane_change_module/src/manager.cpp @@ -169,6 +169,8 @@ void LaneChangeModuleManager::init(rclcpp::Node * node) getOrDeclareParameter(*node, parameter("backward_length_buffer_for_end_of_lane")); p.backward_length_buffer_for_blocking_object = getOrDeclareParameter(*node, parameter("backward_length_buffer_for_blocking_object")); + p.backward_length_from_intersection = + getOrDeclareParameter(*node, parameter("backward_length_from_intersection")); p.lane_changing_lateral_jerk = getOrDeclareParameter(*node, parameter("lane_changing_lateral_jerk")); p.lane_change_prepare_duration = diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 1abbad931cc8b..707596f858ecb 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -73,32 +73,8 @@ void NormalLaneChange::updateLaneChangeStatus() status_.is_ego_in_intersection = utils::lane_change::isWithinIntersection(getRouteHandler(), current_lane, ego_footprint); - if ( - status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane && - status_.path_after_intersection.points.empty()) { - const auto target_neighbor = - utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_); - status_.path_after_intersection = getRouteHandler()->getCenterLinePath( - target_neighbor, 0.0, std::numeric_limits::max()); - status_.distance_from_prev_intersection = 0.0; - } + update_dist_from_intersection(); - if ( - !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane && - !status_.path_after_intersection.points.empty()) { - status_.distance_from_prev_intersection = calcSignedArcLength( - status_.path_after_intersection.points, - status_.path_after_intersection.points.front().point.pose.position, getEgoPosition()); - } - - if ( - !status_.is_ego_in_intersection && !status_.is_ego_in_turn_direction_lane && - status_.distance_from_prev_intersection > - lane_change_parameters_->backward_length_from_intersection && - !status_.path_after_intersection.points.empty()) { - status_.path_after_intersection = PathWithLaneId(); - status_.distance_from_prev_intersection = std::numeric_limits::max(); - } const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -180,24 +156,6 @@ bool NormalLaneChange::isStoppedAtRedTrafficLight() const status_.lane_change_path.info.length.sum()); } -bool NormalLaneChange::is_safe_to_try_change_lanes_at_intersection() const -{ - if (debug_filtered_objects_.target_lane.empty() || status_.target_lanes.empty()) { - return true; - } - - const auto has_overtaking_turn_direction_object = - utils::lane_change::has_overtaking_turn_lane_object( - status_.target_lanes, status_.current_lane_, debug_filtered_objects_.target_lane); - - if (!has_overtaking_turn_direction_object) { - return true; - } - - return status_.distance_from_prev_intersection > - lane_change_parameters_->backward_length_from_intersection; -} - LaneChangePath NormalLaneChange::getLaneChangePath() const { return status_.lane_change_path; @@ -1438,11 +1396,8 @@ bool NormalLaneChange::getLaneChangePaths( if (status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane) { return false; } - if ( - status_.distance_from_prev_intersection < - lane_change_parameters_->backward_length_from_intersection && - utils::lane_change::has_overtaking_turn_lane_object( - status_.target_lanes, status_.current_lane_, target_objects.target_lane)) { + if (utils::lane_change::has_overtaking_turn_lane_object( + status_, *lane_change_parameters_, target_objects.target_lane)) { RCLCPP_DEBUG(logger_, "has overtaking object while ego leaves intersection"); return false; } @@ -1490,6 +1445,12 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const const auto target_objects = getTargetObjects(current_lanes, target_lanes); debug_filtered_objects_ = target_objects; + const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object( + status_, *lane_change_parameters_, target_objects.target_lane); + + if (has_overtaking_object) { + return {false, true}; + } CollisionCheckDebugMap debug_data; const bool is_stuck = isVehicleStuck(current_lanes); const auto safety_status = isLaneChangePathSafe( @@ -1965,4 +1926,40 @@ bool NormalLaneChange::check_prepare_phase() const return check_in_intersection || check_in_turns || check_in_general_lanes; } +void NormalLaneChange::update_dist_from_intersection() +{ + if ( + status_.is_ego_in_intersection && status_.is_ego_in_turn_direction_lane && + path_after_intersection_.empty()) { + const auto target_neighbor = + utils::lane_change::getTargetNeighborLanes(*getRouteHandler(), status_.current_lanes, type_); + auto path_after_intersection = getRouteHandler()->getCenterLinePath( + target_neighbor, 0.0, std::numeric_limits::max()); + path_after_intersection_ = std::move(path_after_intersection.points); + status_.dist_from_prev_intersection = 0.0; + return; + } + + if ( + status_.is_ego_in_intersection || status_.is_ego_in_turn_direction_lane || + path_after_intersection_.empty()) { + return; + } + + const auto & path_points = path_after_intersection_; + const auto & front_point = path_points.front().point.pose.position; + const auto & ego_position = getEgoPosition(); + status_.dist_from_prev_intersection = calcSignedArcLength(path_points, front_point, ego_position); + + if ( + status_.dist_from_prev_intersection >= 0.0 && + status_.dist_from_prev_intersection <= + lane_change_parameters_->backward_length_from_intersection) { + return; + } + + path_after_intersection_.clear(); + status_.dist_from_prev_intersection = std::numeric_limits::max(); +} + } // namespace behavior_path_planner 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 ded41f3cda26c..7dab57bf1e8eb 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1306,6 +1306,16 @@ bool has_blocking_target_object( }); } +bool has_passed_intersection_turn_direction( + const LaneChangeParameters & lc_param, const LaneChangeStatus & status) +{ + if (status.is_ego_in_intersection && status.is_ego_in_turn_direction_lane) { + return false; + } + + return status.dist_from_prev_intersection > lc_param.backward_length_from_intersection; +} + std::vector get_line_string_paths(const ExtendedPredictedObject & object) { const auto transform = [](const auto & predicted_path) -> LineString2d { @@ -1328,11 +1338,21 @@ std::vector get_line_string_paths(const ExtendedPredictedObject & } bool has_overtaking_turn_lane_object( - const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelet & ego_current_lane, + const LaneChangeStatus & status, const LaneChangeParameters & lc_param, const ExtendedPredictedObjects & trailing_objects) { + // Note: This situation is only applicable if the ego is in a turn lane. + if (!has_passed_intersection_turn_direction(lc_param, status)) { + return true; + } + + const auto & target_lanes = status.target_lanes; + const auto & ego_current_lane = status.current_lane; + const auto target_lane_poly = lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); + // to compensate for perception issue, or if object is from behind ego, and tries to overtake, + // but stop all of sudden const auto is_overlap_with_target = [&](const LineString2d & path) { return !boost::geometry::disjoint(path, target_lane_poly); }; From 1c84ddf85825e32517106b497f3cf74bedbf3172 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 5 Nov 2024 19:30:12 +0900 Subject: [PATCH 3/6] fix build error Signed-off-by: Zulfaqar Azmi --- planning/behavior_path_lane_change_module/src/scene.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 707596f858ecb..700929b3933c7 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -64,7 +64,7 @@ void NormalLaneChange::updateLaneChangeStatus() RCLCPP_DEBUG(logger_, "ego's current lane not in route"); return; } - status_.current_lane_ = current_lane; + status_.current_lane = current_lane; const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info); From f9859d4be7697e8966b45ff3e6542209f1c4182c Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 5 Nov 2024 20:43:25 +0900 Subject: [PATCH 4/6] fix logic Signed-off-by: Zulfaqar Azmi --- planning/behavior_path_lane_change_module/src/utils/utils.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 7dab57bf1e8eb..04e2c98815eba 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1342,8 +1342,8 @@ bool has_overtaking_turn_lane_object( const ExtendedPredictedObjects & trailing_objects) { // Note: This situation is only applicable if the ego is in a turn lane. - if (!has_passed_intersection_turn_direction(lc_param, status)) { - return true; + if (has_passed_intersection_turn_direction(lc_param, status)) { + return false; } const auto & target_lanes = status.target_lanes; From 0e156718f0739fe73a1b7ffa9701f9b5641d6499 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 7 Nov 2024 11:50:52 +0900 Subject: [PATCH 5/6] update README Signed-off-by: Zulfaqar Azmi --- planning/behavior_path_lane_change_module/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_lane_change_module/README.md b/planning/behavior_path_lane_change_module/README.md index dc077ab02cae6..29b7f39490699 100644 --- a/planning/behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_lane_change_module/README.md @@ -341,6 +341,7 @@ The following parameters are configurable in `lane_change.param.yaml`. | `prepare_duration` | [m] | double | The preparation time for the ego vehicle to be ready to perform lane change. | 4.0 | | `backward_length_buffer_for_end_of_lane` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change | 3.0 | | `backward_length_buffer_for_blocking_object` | [m] | double | The end of lane buffer to ensure ego vehicle has enough distance to start lane change when there is an object in front | 3.0 | +| `backward_length_from_intersection` | [m] | double | Distance threshold from the last intersection to invalidate or cancel the lane change path | 5.0 | | `lane_change_finish_judge_buffer` | [m] | double | The additional buffer used to confirm lane change process completion | 3.0 | | `finish_judge_lateral_threshold` | [m] | double | Lateral distance threshold to confirm lane change process completion | 0.2 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | From ad79c78e2d56e40f755d79d8d92c44efee4f59ca Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 7 Nov 2024 12:02:30 +0900 Subject: [PATCH 6/6] change variable name Signed-off-by: Zulfaqar Azmi --- .../include/behavior_path_lane_change_module/utils/path.hpp | 2 +- planning/behavior_path_lane_change_module/src/scene.cpp | 2 +- planning/behavior_path_lane_change_module/src/utils/utils.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 f4fd6d4816413..913077eac0413 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 @@ -43,7 +43,7 @@ struct LaneChangeStatus LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; - lanelet::ConstLanelet current_lane{}; + lanelet::ConstLanelet ego_lane{}; std::vector lane_follow_lane_ids{}; std::vector lane_change_lane_ids{}; bool is_safe{false}; diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 700929b3933c7..69c4a939767bb 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -64,7 +64,7 @@ void NormalLaneChange::updateLaneChangeStatus() RCLCPP_DEBUG(logger_, "ego's current lane not in route"); return; } - status_.current_lane = current_lane; + status_.ego_lane = current_lane; const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), getCommonParam().vehicle_info); 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 04e2c98815eba..56cb451ce8f14 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1347,7 +1347,7 @@ bool has_overtaking_turn_lane_object( } const auto & target_lanes = status.target_lanes; - const auto & ego_current_lane = status.current_lane; + const auto & ego_current_lane = status.ego_lane; const auto target_lane_poly = lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon();