From f6de32d656fbe80200b245137d315a0e2468febd Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 5 Nov 2024 19:21:45 +0900 Subject: [PATCH] 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); };