feat(lane_change): ensure LC merging lane stop point is safe (#8369)

* function to check for merging lane Signed-off-by: mohammad alqudah <[email protected]> * function to compute distance from last fit width center line point to lane end Signed-off-by: mohammad alqudah <[email protected]> * ensure lane width at LC stop point is larger than ego width Signed-off-by: mohammad alqudah <[email protected]> * refactor function isMergingLane Signed-off-by: mohammad alqudah <[email protected]> * improve implementation Signed-off-by: mohammad alqudah <[email protected]> * apply logic only when current ego foot print is within lane Signed-off-by: mohammad alqudah <[email protected]> * change implementation to use intersection points of buffered centerline and lane polygon Signed-off-by: mohammad alqudah <[email protected]> * minor refactoring Signed-off-by: mohammad alqudah <[email protected]> * overload function isEgoWithinOriginalLane to pass lane polygon directly Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]>
autowarefoundation · Aug 16, 2024 · 0c28bc9 · 0c28bc9
1 parent 77059a1
commit 0c28bc9
Show file tree

Hide file tree

Showing 5 changed files with 114 additions and 24 deletions.
diff --git a/...ane_change_module/include/autoware/behavior_path_lane_change_module/utils/calculation.hpp b/...ane_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
@@ -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/...behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/...behavior_path_planner/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp
@@ -16,6 +16,8 @@
 #include <autoware/behavior_path_lane_change_module/utils/calculation.hpp>
 #include <autoware/behavior_path_planner_common/utils/utils.hpp>
 
+#include <boost/geometry/algorithms/buffer.hpp>
+
 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<double>(buffer_distance),
+    strategy::side_straight(), strategy::join_miter(), strategy::end_flat(),
+    strategy::point_square());
+
+  if (center_line_polygon.empty()) return 0.0;
+
+  std::vector<universe_utils::Point2d> 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<double>::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/...ehavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/utils.hpp b/...ehavior_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<PathWithLaneId> 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<double> 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
@@ -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<double> 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);
   }