From 98f9724d4fec3ea3b1d834bca45569253f8dfc3d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Thu, 7 Nov 2024 19:39:23 +0900 Subject: [PATCH] fix(lane_change): enable cancel when ego in turn direction lane (RT0-33893) (#1594) * yield at intersection by making the check conservative Signed-off-by: Zulfaqar Azmi * make code of similar to main branch Signed-off-by: Zulfaqar Azmi * fix build error Signed-off-by: Zulfaqar Azmi * fix logic Signed-off-by: Zulfaqar Azmi * update README Signed-off-by: Zulfaqar Azmi * change variable name Signed-off-by: Zulfaqar Azmi --------- Signed-off-by: Zulfaqar Azmi --- .../README.md | 1 + .../config/lane_change.param.yaml | 2 + .../scene.hpp | 3 + .../utils/data_structs.hpp | 1 + .../utils/path.hpp | 5 ++ .../utils/utils.hpp | 10 +++ .../src/interface.cpp | 1 + .../src/manager.cpp | 2 + .../src/scene.cpp | 72 +++++++++++++++++++ .../src/utils/utils.cpp | 67 +++++++++++++++++ 10 files changed, 164 insertions(+) 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 | 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 04f21f5249240..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 @@ -186,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/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..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 @@ -21,6 +21,7 @@ #include +#include #include namespace behavior_path_planner @@ -42,11 +43,15 @@ struct LaneChangeStatus LaneChangePath lane_change_path{}; lanelet::ConstLanelets current_lanes{}; lanelet::ConstLanelets target_lanes{}; + lanelet::ConstLanelet ego_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 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 48b940093c2cd..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 @@ -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,14 @@ 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); + +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 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 61699a8b35bdc..c734e845d850c 100644 --- a/planning/behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_lane_change_module/src/interface.cpp @@ -120,6 +120,7 @@ ModuleStatus LaneChangeInterface::updateState() module_type_->evaluateApprovedPathWithUnsafeHysteresis(module_type_->isApprovedPathSafe()); setObjectDebugVisualization(); + 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 284ea9409e924..69c4a939767bb 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -54,6 +54,27 @@ 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_.ego_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); + + update_dist_from_intersection(); + const auto [found_valid_path, found_safe_path] = getSafePath(status_.lane_change_path); // Update status @@ -1372,6 +1393,15 @@ 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 (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; + } + std::vector filtered_objects = filterObjectsInTargetLane(target_objects, target_lanes); if ( @@ -1415,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( @@ -1890,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 0d5b20db3c0c8..56cb451ce8f14 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,71 @@ bool has_blocking_target_object( return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } + +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 { + 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 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 false; + } + + const auto & target_lanes = status.target_lanes; + const auto & ego_current_lane = status.ego_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); + }; + + 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