diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md index a52d842b5dcb9..3b23dd8b4212b 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md @@ -823,6 +823,7 @@ The following parameters are configurable in [lane_change.param.yaml](https://gi | `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 | 2.0 | | `lane_changing_lateral_jerk` | [m/s3] | double | Lateral jerk value for lane change path generation | 0.5 | | `minimum_lane_changing_velocity` | [m/s] | double | Minimum speed during lane changing process. | 2.78 | diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml index 0a2c9991ec5fd..a2a6fbfd880e4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml @@ -6,6 +6,7 @@ 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_changing_lateral_jerk: 0.5 # [m/s3] 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 5334af7ab6e8e..4c8cbe05f5535 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 @@ -211,6 +211,9 @@ class NormalLaneChange : public LaneChangeBase return common_data_ptr_->lanes_ptr->target; } + void update_dist_from_intersection(); + + std::vector path_after_intersection_; double stop_time_{0.0}; static constexpr double floating_err_th{1e-3}; }; 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 5178d76aca29e..d970976d80a8d 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 @@ -39,6 +39,7 @@ using geometry_msgs::msg::Twist; using nav_msgs::msg::Odometry; using route_handler::Direction; using route_handler::RouteHandler; +using universe_utils::Polygon2d; using utils::path_safety_checker::ExtendedPredictedObjects; struct LateralAccelerationMap @@ -110,6 +111,7 @@ struct Parameters // lane change parameters double backward_length_buffer_for_end_of_lane{0.0}; double backward_length_buffer_for_blocking_object{0.0}; + 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}; @@ -220,6 +222,7 @@ struct Lanes { bool current_lane_in_goal_section{false}; bool target_lane_in_goal_section{false}; + lanelet::ConstLanelet ego_lane; lanelet::ConstLanelets current; lanelet::ConstLanelets target_neighbor; lanelet::ConstLanelets target; @@ -323,6 +326,8 @@ struct MinMaxValue struct TransientData { + Polygon2d current_footprint; // ego's polygon at current pose + MinMaxValue lane_changing_length; // lane changing length for a single lane change MinMaxValue current_dist_buffer; // distance buffer computed backward from current lanes' terminal end @@ -331,6 +336,8 @@ struct TransientData double dist_to_terminal_end{ std::numeric_limits::min()}; // distance from ego base link to the current lanes' // terminal end + double dist_from_prev_intersection{std::numeric_limits::max()}; + // terminal end double dist_to_terminal_start{ std::numeric_limits::min()}; // distance from ego base link to the current lanes' // terminal start @@ -347,6 +354,9 @@ struct TransientData bool is_ego_near_current_terminal_start{false}; bool is_ego_stuck{false}; + + bool in_turn_direction_lane{false}; + bool in_intersection{false}; }; using RouteHandlerPtr = std::shared_ptr; 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 18fa418a0970d..4bd5d1e53ed6c 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 @@ -44,6 +44,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker:: using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped; using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon; using autoware::route_handler::Direction; +using autoware::universe_utils::LineString2d; using autoware::universe_utils::Polygon2d; using autoware::vehicle_info_utils::VehicleInfo; using autoware_perception_msgs::msg::PredictedObject; @@ -188,13 +189,12 @@ rclcpp::Logger getLogger(const std::string & type); * The footprint is determined by the vehicle's pose and its dimensions, including the distance * from the base to the front and rear ends of the vehicle, as well as its width. * - * @param ego_pose The current pose of the ego vehicle. - * @param ego_info The structural information of the ego vehicle, such as its maximum longitudinal - * offset, rear overhang, and width. + * @param common_data_ptr Shared pointer to CommonData that holds necessary ego vehicle's dimensions + * and pose information. * * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle. */ -Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info); +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info); Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left); @@ -213,7 +213,7 @@ Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, boo * * @return bool True if the polygon is within the intersection area, false otherwise. */ -bool isWithinIntersection( +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); @@ -230,7 +230,8 @@ bool isWithinIntersection( * @return bool True if the polygon is within a lane designated for turning, false if it is within a * straight lane or no turn direction is specified. */ -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr); @@ -297,6 +298,51 @@ double get_min_dist_to_current_lanes_obj( bool has_blocking_target_object( const CommonDataPtr & common_data_ptr, const FilteredByLanesExtendedObjects & filtered_objects, const double stop_arc_length, const PathWithLaneId & path); -} // namespace autoware::behavior_path_planner::utils::lane_change +/** + * @brief Checks if the ego vehicle has passed any turn direction within an intersection. + * + * This function determines whether the ego vehicle has exited the intersection and + * turn lane area based on its distance from the previous intersection. It considers + * whether the ego vehicle is currently in an intersection and a turn lane. + * + * @param common_data_ptr Shared pointer to CommonData containing the transient data and + * lane-change parameters required for the distance's comparison. + * + * @return true if the ego vehicle has passed the intersection turn direction, false otherwise. + */ +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr); + +/** + * @brief Retrieves the predicted paths of an object as 2D line strings. + * + * This function transforms each predicted path of an object into a LineString2d, representing + * a 2D sequence of points. Each point in the path is extracted from the predicted path's + * position and converted to a 2D point. + * + * @param object The predicted object whose paths will be converted into 2D line strings. + * + * @return std::vector A vector of 2D line strings representing the predicted paths + * of the object. + */ +std::vector get_line_string_paths(const ExtendedPredictedObject & object); + +/** + * @brief Determines if there is an object in the turn lane that could overtake the ego vehicle. + * + * This function checks for any trailing objects in the turn lane that may attempt to overtake + * the ego vehicle. The check is only applicable if the ego vehicle is still within a certain + * distance from the previous intersection's turn lane. It evaluates whether any of the predicted + * paths or the initial polygon of trailing objects overlap with the target lane polygon. + * + * @param common_data_ptr Shared pointer to CommonData containing lane and polygon information + * for the ego vehicle. + * @param trailing_objects A collection of predicted objects trailing the ego vehicle. + * + * @return true if there is an object in the turn lane with a potential to overtake, false + * otherwise. + */ +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects); +} // namespace autoware::behavior_path_planner::utils::lane_change #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_ diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp index db6acc9ecd05b..04371e8ff3f37 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/manager.cpp @@ -177,6 +177,8 @@ LCParamPtr LaneChangeModuleManager::set_params(rclcpp::Node * node, const std::s 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_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 d6362aed5247e..aea6cda6c6807 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 @@ -75,6 +75,13 @@ void NormalLaneChange::update_lanes(const bool is_approved) return; } + lanelet::ConstLanelet current_lane; + if (!common_data_ptr_->route_handler_ptr->getClosestLaneletWithinRoute( + common_data_ptr_->get_ego_pose(), ¤t_lane)) { + return; + } + + common_data_ptr_->lanes_ptr->ego_lane = current_lane; const auto is_same_lanes_with_prev_iteration = utils::lane_change::is_same_lane_with_prev_iteration( common_data_ptr_, current_lanes, target_lanes); @@ -93,6 +100,8 @@ void NormalLaneChange::update_lanes(const bool is_approved) common_data_ptr_->lanes_ptr->target_neighbor = utils::lane_change::get_target_neighbor_lanes( *route_handler_ptr, current_lanes, common_data_ptr_->lc_type); + common_data_ptr_->current_lanes_path = + route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits::max()); common_data_ptr_->lanes_ptr->current_lane_in_goal_section = route_handler_ptr->isInGoalRouteSection(current_lanes.back()); common_data_ptr_->lanes_ptr->target_lane_in_goal_section = @@ -150,6 +159,18 @@ void NormalLaneChange::update_transient_data() transient_data.is_ego_near_current_terminal_start = transient_data.dist_to_terminal_start < transient_data.max_prepare_length; + transient_data.current_footprint = utils::lane_change::get_ego_footprint( + common_data_ptr_->get_ego_pose(), common_data_ptr_->bpp_param_ptr->vehicle_info); + + const auto & ego_lane = common_data_ptr_->lanes_ptr->ego_lane; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; + transient_data.in_intersection = utils::lane_change::is_within_intersection( + route_handler_ptr, ego_lane, transient_data.current_footprint); + transient_data.in_turn_direction_lane = + utils::lane_change::is_within_turn_direction_lanes(ego_lane, transient_data.current_footprint); + + update_dist_from_intersection(); + updateStopTime(); transient_data.is_ego_stuck = is_ego_stuck(); @@ -754,6 +775,10 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + if (common_data_ptr_->transient_data.in_turn_direction_lane) { + return true; + } + const auto nearest_idx = autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( status_.lane_change_path.path.points, getEgoPose(), planner_data_->parameters.ego_nearest_dist_threshold, @@ -1404,6 +1429,11 @@ bool NormalLaneChange::check_candidate_path_safety( const LaneChangePath & candidate_path, const lane_change::TargetObjects & target_objects) const { const auto is_stuck = common_data_ptr_->transient_data.is_ego_stuck; + if (utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing)) { + throw std::logic_error("Ego is nearby intersection, and there might be overtaking vehicle."); + } + if ( !is_stuck && !utils::lane_change::passed_parked_objects( common_data_ptr_, candidate_path, filtered_objects_.target_lane_leading, @@ -1579,6 +1609,13 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const CollisionCheckDebugMap debug_data; + const auto has_overtaking_object = utils::lane_change::has_overtaking_turn_lane_object( + common_data_ptr_, filtered_objects_.target_lane_trailing); + + if (has_overtaking_object) { + return {false, true}; + } + const auto has_passed_parked_objects = utils::lane_change::passed_parked_objects( common_data_ptr_, path, filtered_objects_.target_lane_leading, debug_data); @@ -2076,7 +2113,6 @@ void NormalLaneChange::updateStopTime() bool NormalLaneChange::check_prepare_phase() const { const auto & route_handler = getRouteHandler(); - const auto & vehicle_info = getCommonParam().vehicle_info; const auto check_in_general_lanes = lane_change_parameters_->enable_collision_check_for_prepare_phase_in_general_lanes; @@ -2089,24 +2125,52 @@ bool NormalLaneChange::check_prepare_phase() const return check_in_general_lanes; } - const auto ego_footprint = utils::lane_change::getEgoCurrentFootprint(getEgoPose(), vehicle_info); + const auto check_in_intersection = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection && + common_data_ptr_->transient_data.in_intersection; - const auto check_in_intersection = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_intersection) { - return false; - } + const auto check_in_turns = + lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns && + common_data_ptr_->transient_data.in_turn_direction_lane; - return utils::lane_change::isWithinIntersection(route_handler, current_lane, ego_footprint); - }); + return check_in_intersection || check_in_turns || check_in_general_lanes; +} - const auto check_in_turns = std::invoke([&]() { - if (!lane_change_parameters_->enable_collision_check_for_prepare_phase_in_turns) { - return false; - } +void NormalLaneChange::update_dist_from_intersection() +{ + auto & transient_data = common_data_ptr_->transient_data; + const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr; - return utils::lane_change::isWithinTurnDirectionLanes(current_lane, ego_footprint); - }); + if ( + transient_data.in_intersection && transient_data.in_turn_direction_lane && + path_after_intersection_.empty()) { + auto path_after_intersection = route_handler_ptr->getCenterLinePath( + common_data_ptr_->lanes_ptr->target_neighbor, 0.0, std::numeric_limits::max()); + path_after_intersection_ = std::move(path_after_intersection.points); + transient_data.dist_from_prev_intersection = 0.0; + return; + } - return check_in_intersection || check_in_turns || check_in_general_lanes; + if ( + transient_data.in_intersection || transient_data.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 = common_data_ptr_->get_ego_pose().position; + transient_data.dist_from_prev_intersection = + calcSignedArcLength(path_points, front_point, ego_position); + + if ( + transient_data.dist_from_prev_intersection >= 0.0 && + transient_data.dist_from_prev_intersection <= + common_data_ptr_->lc_param_ptr->backward_length_from_intersection) { + return; + } + + path_after_intersection_.clear(); + transient_data.dist_from_prev_intersection = std::numeric_limits::max(); } } // namespace autoware::behavior_path_planner 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 455792733ac97..fb7b201c7cd55 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 @@ -974,8 +974,7 @@ rclcpp::Logger getLogger(const std::string & type) return rclcpp::get_logger("lane_change").get_child(type); } -Polygon2d getEgoCurrentFootprint( - const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info) +Polygon2d get_ego_footprint(const Pose & ego_pose, const VehicleInfo & ego_info) { const auto base_to_front = ego_info.max_longitudinal_offset_m; const auto base_to_rear = ego_info.rear_overhang_m; @@ -992,22 +991,22 @@ Point getEgoFrontVertex( return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position; } -bool isWithinIntersection( +bool is_within_intersection( const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string id = lanelet.attributeOr("intersection_area", "else"); - if (id == "else") { + if (id == "else" || !std::atoi(id.c_str())) { return false; } - if (!std::atoi(id.c_str())) { + if (!route_handler || !route_handler->getLaneletMapPtr()) { return false; } - const auto lanelet_polygon_opt = - route_handler->getLaneletMapPtr()->polygonLayer.find(std::atoi(id.c_str())); - if (lanelet_polygon_opt == route_handler->getLaneletMapPtr()->polygonLayer.end()) { + const auto & polygon_layer = route_handler->getLaneletMapPtr()->polygonLayer; + const auto lanelet_polygon_opt = polygon_layer.find(std::atoi(id.c_str())); + if (lanelet_polygon_opt == polygon_layer.end()) { return false; } const auto & lanelet_polygon = *lanelet_polygon_opt; @@ -1016,7 +1015,8 @@ bool isWithinIntersection( polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet_polygon.basicPolygon()))); } -bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) +bool is_within_turn_direction_lanes( + const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon) { const std::string turn_direction = lanelet.attributeOr("turn_direction", "else"); if (turn_direction == "else" || turn_direction == "straight") { @@ -1101,9 +1101,9 @@ bool is_ahead_of_ego( return dist_to_base_link >= 0.0; } - const auto ego_polygon = getEgoCurrentFootprint(current_ego_pose, ego_info).outer(); + const auto & current_footprint = common_data_ptr->transient_data.current_footprint.outer(); auto ego_min_dist_to_end = std::numeric_limits::max(); - for (const auto & ego_edge_point : ego_polygon) { + for (const auto & ego_edge_point : current_footprint) { const auto ego_edge = autoware::universe_utils::createPoint(ego_edge_point.x(), ego_edge_point.y(), 0.0); const auto dist_to_end = autoware::motion_utils::calcSignedArcLength( @@ -1270,4 +1270,64 @@ bool has_blocking_target_object( return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } + +bool has_passed_intersection_turn_direction(const CommonDataPtr & common_data_ptr) +{ + const auto & transient_data = common_data_ptr->transient_data; + if (transient_data.in_intersection && transient_data.in_turn_direction_lane) { + return false; + } + + return transient_data.dist_from_prev_intersection > + common_data_ptr->lc_param_ptr->backward_length_from_intersection; +} + +std::vector get_line_string_paths(const ExtendedPredictedObject & object) +{ + const auto to_linestring_2d = [](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 = universe_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), to_linestring_2d); + + return line_strings; +} + +bool has_overtaking_turn_lane_object( + const CommonDataPtr & common_data_ptr, const ExtendedPredictedObjects & trailing_objects) +{ + // Note: This situation is only applicable if the ego is in a turn lane. + if (has_passed_intersection_turn_direction(common_data_ptr)) { + return false; + } + + const auto is_path_overlap_with_target = [&](const LineString2d & path) { + return !boost::geometry::disjoint(path, common_data_ptr->lanes_polygon_ptr->target.value()); + }; + + const auto is_object_overlap_with_target = [&](const auto & object) { + // to compensate for perception issue, or if object is from behind ego, and tries to overtake, + // but stop all of sudden + if (!boost::geometry::disjoint( + object.initial_polygon, common_data_ptr->lanes_polygon_ptr->current.value())) { + return true; + } + + const auto paths = get_line_string_paths(object); + return std::any_of(paths.begin(), paths.end(), is_path_overlap_with_target); + }; + + return std::any_of( + trailing_objects.begin(), trailing_objects.end(), is_object_overlap_with_target); +} } // namespace autoware::behavior_path_planner::utils::lane_change