From 0c28bc9ae397a3d28740c1ebc1e67b898352574e Mon Sep 17 00:00:00 2001 From: mkquda <168697710+mkquda@users.noreply.github.com> Date: Fri, 16 Aug 2024 16:26:53 +0900 Subject: [PATCH] feat(lane_change): ensure LC merging lane stop point is safe (#8369) * function to check for merging lane Signed-off-by: mohammad alqudah * function to compute distance from last fit width center line point to lane end Signed-off-by: mohammad alqudah * ensure lane width at LC stop point is larger than ego width Signed-off-by: mohammad alqudah * refactor function isMergingLane Signed-off-by: mohammad alqudah * improve implementation Signed-off-by: mohammad alqudah * apply logic only when current ego foot print is within lane Signed-off-by: mohammad alqudah * change implementation to use intersection points of buffered centerline and lane polygon Signed-off-by: mohammad alqudah * minor refactoring Signed-off-by: mohammad alqudah * overload function isEgoWithinOriginalLane to pass lane polygon directly Signed-off-by: mohammad alqudah --------- Signed-off-by: mohammad alqudah --- .../utils/calculation.hpp | 17 +++++++ .../src/scene.cpp | 16 +++++-- .../src/utils/calculation.cpp | 47 ++++++++++++++++++ .../utils/utils.hpp | 10 +++- .../src/utils/utils.cpp | 48 +++++++++++-------- 5 files changed, 114 insertions(+), 24 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp index 3dc5e7ee62a57..6c48445b47567 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp @@ -58,6 +58,23 @@ double calc_dist_from_pose_to_terminal_end( * @return The required backward buffer distance in meters. */ double calc_stopping_distance(const LCParamPtr & lc_param_ptr); + +/** + * @brief Calculates the distance to last fit width position along the lane + * + * This function computes the distance from the current ego position to the last + * position along the lane where the ego foot prints stays within the lane + * boundaries. + * + * @param lanelets current ego lanelets + * @param src_pose source pose to calculate distance from + * @param bpp_param common parameters used in behavior path planner. + * @param margin additional margin for checking lane width + * @return distance to last fit width position along the lane + */ +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin = 0.1); } // namespace autoware::behavior_path_planner::utils::lane_change::calculation #endif // AUTOWARE__BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ 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 c71c971e7b51f..23925d56badb3 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 @@ -397,6 +397,14 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = filterObjects(); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); + if (utils::isEgoWithinOriginalLane(curr_lanes_poly, getEgoPose(), planner_data_->parameters)) { + const double distance_to_last_fit_width = + utils::lane_change::calculation::calc_dist_to_last_fit_width( + lanelets, path.points.front().point.pose, planner_data_->parameters); + stopping_distance = std::min(stopping_distance, distance_to_last_fit_width); + } + const auto & lc_start_point = status_.lane_change_path.info.lane_changing_start; if (!is_valid_start_point(common_data_ptr_, lc_start_point)) { @@ -737,8 +745,9 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const return false; } + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); if (!utils::isEgoWithinOriginalLane( - get_current_lanes(), getEgoPose(), planner_data_->parameters, + curr_lanes_poly, getEgoPose(), planner_data_->parameters, lane_change_parameters_->cancel.overhang_tolerance)) { lane_change_debug_.is_able_to_return_to_current_lane = false; return false; @@ -759,7 +768,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( - get_current_lanes(), estimated_pose, planner_data_->parameters, + curr_lanes_poly, 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; @@ -817,13 +826,14 @@ bool NormalLaneChange::isAbleToStopSafely() const const auto stop_dist = -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto & curr_lanes_poly = common_data_ptr_->lanes_polygon_ptr->current.value(); double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { dist += calcSignedArcLength(status_.lane_change_path.path.points, idx, idx + 1); if (dist > stop_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - get_current_lanes(), estimated_pose, planner_data_->parameters); + curr_lanes_poly, estimated_pose, planner_data_->parameters); } } return true; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp index ac205ceedb34b..8f04395be572a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -16,6 +16,8 @@ #include #include +#include + namespace autoware::behavior_path_planner::utils::lane_change::calculation { double calc_ego_dist_to_terminal_end(const CommonDataPtr & common_data_ptr) @@ -54,4 +56,49 @@ double calc_stopping_distance(const LCParamPtr & lc_param_ptr) const auto param_back_dist = lc_param_ptr->backward_length_buffer_for_end_of_lane; return std::max(min_back_dist, param_back_dist); } + +double calc_dist_to_last_fit_width( + const lanelet::ConstLanelets lanelets, const Pose & src_pose, + const BehaviorPathPlannerParameters & bpp_param, const double margin) +{ + if (lanelets.empty()) return 0.0; + + const auto lane_polygon = lanelets.back().polygon2d().basicPolygon(); + const auto center_line = lanelet::utils::generateFineCenterline(lanelets.back(), 1.0); + + if (center_line.size() <= 1) return 0.0; + + universe_utils::LineString2d line_string; + line_string.reserve(center_line.size() - 1); + std::for_each(center_line.begin() + 1, center_line.end(), [&line_string](const auto & point) { + boost::geometry::append(line_string, universe_utils::Point2d(point.x(), point.y())); + }); + + const double buffer_distance = 0.5 * bpp_param.vehicle_width + margin; + universe_utils::MultiPolygon2d center_line_polygon; + namespace strategy = boost::geometry::strategy::buffer; + boost::geometry::buffer( + line_string, center_line_polygon, strategy::distance_symmetric(buffer_distance), + strategy::side_straight(), strategy::join_miter(), strategy::end_flat(), + strategy::point_square()); + + if (center_line_polygon.empty()) return 0.0; + + std::vector intersection_points; + boost::geometry::intersection(lane_polygon, center_line_polygon, intersection_points); + + if (intersection_points.empty()) { + return utils::getDistanceToEndOfLane(src_pose, lanelets); + } + + Pose pose; + double distance = std::numeric_limits::max(); + for (const auto & point : intersection_points) { + pose.position.x = boost::geometry::get<0>(point); + pose.position.y = boost::geometry::get<1>(point); + distance = std::min(distance, utils::getSignedDistance(src_pose, pose, lanelets)); + } + + return std::max(distance - (bpp_param.base_link2front + margin), 0.0); +} } // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp index 5c1eee92c2a5d..03e6c2d7f8167 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp @@ -55,6 +55,8 @@ using geometry_msgs::msg::Vector3; using tier4_planning_msgs::msg::PathPointWithLaneId; using tier4_planning_msgs::msg::PathWithLaneId; +static constexpr double eps = 0.01; + struct PolygonPoint { geometry_msgs::msg::Point point; @@ -241,6 +243,10 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); + // path management // TODO(Horibe) There is a similar function in route_handler. Check. @@ -249,8 +255,10 @@ std::shared_ptr generateCenterLinePath( PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path); +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, const bool left_side); double getSignedDistanceFromBoundary( - const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose, const bool left_side); + const lanelet::ConstLanelets & lanelets, const Pose & pose, const bool left_side); std::optional getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const double vehicle_width, const double base_link2front, const double base_link2rear, const Pose & vehicle_pose, const bool left_side); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp index aa16f1bb23b79..0f3751cc32dd3 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/utils.cpp @@ -559,19 +559,24 @@ bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const BehaviorPathPlannerParameters & common_param, const double outer_margin) { - const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); - const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); - const auto base_link2front = common_param.base_link2front; - const auto base_link2rear = common_param.base_link2rear; - const auto vehicle_width = common_param.vehicle_width; + const auto combined_lane = lanelet::utils::combineLaneletsShape(current_lanes); + const auto lane_polygon = combined_lane.polygon2d().basicPolygon(); + return isEgoWithinOriginalLane(lane_polygon, current_pose, common_param, outer_margin); +} + +bool isEgoWithinOriginalLane( + const lanelet::BasicPolygon2d & lane_polygon, const Pose & current_pose, + const BehaviorPathPlannerParameters & common_param, const double outer_margin) +{ const auto vehicle_poly = autoware::universe_utils::toFootprint( - current_pose, base_link2front, base_link2rear, vehicle_width); + current_pose, common_param.base_link2front, common_param.base_link2rear, + common_param.vehicle_width); // Check if the ego vehicle is entirely within the lane with a given outer margin. for (const auto & p : vehicle_poly.outer()) { // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a // positive value. - const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + const auto dist = boost::geometry::distance(p, lane_polygon); if (dist > std::max(outer_margin, 0.0)) { return false; // out of polygon } @@ -816,25 +821,29 @@ PathPointWithLaneId insertStopPoint(const double length, PathWithLaneId & path) return path.points.at(*insert_idx); } +double getSignedDistanceFromLaneBoundary( + const lanelet::ConstLanelet & lanelet, const Point & position, bool left_side) +{ + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(position); + const auto & boundary_line_2d = left_side ? lanelet.leftBound2d() : lanelet.rightBound2d(); + const auto arc_coordinates = lanelet::geometry::toArcCoordinates( + boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); + return arc_coordinates.distance; +} + double getSignedDistanceFromBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose, bool left_side) { lanelet::ConstLanelet closest_lanelet; - lanelet::ArcCoordinates arc_coordinates; + if (lanelet::utils::query::getClosestLanelet(lanelets, pose, &closest_lanelet)) { - const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); - const auto & boundary_line_2d = left_side - ? lanelet::utils::to2D(closest_lanelet.leftBound3d()) - : lanelet::utils::to2D(closest_lanelet.rightBound3d()); - arc_coordinates = lanelet::geometry::toArcCoordinates( - boundary_line_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("utils"), - "closest shoulder lanelet not found."); + return getSignedDistanceFromLaneBoundary(closest_lanelet, pose.position, left_side); } - return arc_coordinates.distance; + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utils"), "closest lanelet not found."); + + return 0.0; } std::optional getSignedDistanceFromBoundary( @@ -1211,7 +1220,6 @@ PathWithLaneId setDecelerationVelocity( const auto stop_point_length = autoware::motion_utils::calcSignedArcLength(reference_path.points, 0, target_pose.position) + buffer; - constexpr double eps{0.01}; if (std::abs(target_velocity) < eps && stop_point_length > 0.0) { const auto stop_point = utils::insertStopPoint(stop_point_length, reference_path); }