feat(lane_change): add checks to ensure the edge of vehicle do not ex…

…ceed target lane boundary when changing lanes (autowarefoundation#8750) * check if LC candidate path footprint exceeds target lane far bound Signed-off-by: mohammad alqudah <[email protected]> * add parameter to enable/disable check Signed-off-by: mohammad alqudah <[email protected]> * check only lane changing section of cadidate path Signed-off-by: mohammad alqudah <[email protected]> * fix spelling Signed-off-by: mohammad alqudah <[email protected]> * small refactoring Signed-off-by: mohammad alqudah <[email protected]> --------- Signed-off-by: mohammad alqudah <[email protected]>
tier4 · Dec 19, 2024 · da8a0f8 · da8a0f8
1 parent b9d569e
commit da8a0f8
Show file tree

Hide file tree

Showing 6 changed files with 61 additions and 4 deletions.
diff --git a/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml b/...vior_path_planner/autoware_behavior_path_lane_change_module/config/lane_change.param.yaml
@@ -34,6 +34,7 @@
       # safety check
       safety_check:
         allow_loose_check_for_cancel: true
+        enable_target_lane_bound_check: true
         collision_check_yaw_diff_threshold: 3.1416
         execution:
           expected_front_deceleration: -1.0

diff --git a/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp b/...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
@@ -154,6 +154,7 @@ struct Parameters
 
   // safety check
   bool allow_loose_check_for_cancel{true};
+  bool enable_target_lane_bound_check{true};
   double collision_check_yaw_diff_threshold{3.1416};
   utils::path_safety_checker::RSSparams rss_params{};
   utils::path_safety_checker::RSSparams rss_params_for_parked{};

diff --git a/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp b/...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
@@ -45,6 +45,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PoseWithVeloc
 using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
 using autoware::route_handler::Direction;
 using autoware::universe_utils::Polygon2d;
+using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_perception_msgs::msg::PredictedPath;
@@ -99,6 +100,10 @@ bool isPathInLanelets(
   const PathWithLaneId & path, const lanelet::ConstLanelets & current_lanes,
   const lanelet::ConstLanelets & target_lanes);
 
+bool pathFootprintExceedsTargetLaneBound(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
+  const double margin = 0.1);
+
 std::optional<LaneChangePath> constructCandidatePath(
   const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info,
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
@@ -216,8 +221,9 @@ rclcpp::Logger getLogger(const std::string & type);
  *
  * @return Polygon2d A polygon representing the current 2D footprint of the ego vehicle.
  */
-Polygon2d getEgoCurrentFootprint(
-  const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info);
+Polygon2d getEgoCurrentFootprint(const Pose & ego_pose, const VehicleInfo & ego_info);
+
+Point getEgoFrontVertex(const Pose & ego_pose, const VehicleInfo & ego_info, bool left);
 
 /**
  * @brief Checks if the given polygon is within an intersection area.
@@ -303,8 +309,8 @@ namespace autoware::behavior_path_planner::utils::lane_change::debug
 geometry_msgs::msg::Point32 create_point32(const geometry_msgs::msg::Pose & pose);
 
 geometry_msgs::msg::Polygon createExecutionArea(
-  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const Pose & pose,
-  double additional_lon_offset, double additional_lat_offset);
+  const VehicleInfo & vehicle_info, const Pose & pose, double additional_lon_offset,
+  double additional_lat_offset);
 }  // namespace autoware::behavior_path_planner::utils::lane_change::debug
 
 #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
@@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node)
   // safety check
   p.allow_loose_check_for_cancel =
     getOrDeclareParameter<bool>(*node, parameter("safety_check.allow_loose_check_for_cancel"));
+  p.enable_target_lane_bound_check =
+    getOrDeclareParameter<bool>(*node, parameter("safety_check.enable_target_lane_bound_check"));
   p.collision_check_yaw_diff_threshold = getOrDeclareParameter<double>(
     *node, parameter("safety_check.collision_check_yaw_diff_threshold"));
 

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
@@ -1675,6 +1675,15 @@ bool NormalLaneChange::getLaneChangePaths(
           return false;
         }
 
+        if (
+          lane_change_parameters_->enable_target_lane_bound_check &&
+          utils::lane_change::pathFootprintExceedsTargetLaneBound(
+            common_data_ptr_, candidate_path.value().shifted_path.path,
+            common_parameters.vehicle_info)) {
+          debug_print_lat("Reject: Path footprint exceeds target lane boundary. Skip lane change.");
+          return false;
+        }
+
         const auto is_safe = std::invoke([&]() {
           constexpr size_t decel_sampling_num = 1;
           const auto safety_check_with_normal_rss = isLaneChangePathSafe(

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
@@ -241,6 +241,36 @@ bool isPathInLanelets(
   return true;
 }
 
+bool pathFootprintExceedsTargetLaneBound(
+  const CommonDataPtr & common_data_ptr, const PathWithLaneId & path, const VehicleInfo & ego_info,
+  const double margin)
+{
+  if (common_data_ptr->direction == Direction::NONE || path.points.empty()) {
+    return false;
+  }
+
+  const auto & target_lanes = common_data_ptr->lanes_ptr->target;
+  const bool is_left = common_data_ptr->direction == Direction::LEFT;
+
+  const auto combined_target_lane = lanelet::utils::combineLaneletsShape(target_lanes);
+
+  for (const auto & path_point : path.points) {
+    const auto & pose = path_point.point.pose;
+    const auto front_vertex = getEgoFrontVertex(pose, ego_info, is_left);
+
+    const auto sign = is_left ? -1.0 : 1.0;
+    const auto dist_to_boundary =
+      sign * utils::getSignedDistanceFromLaneBoundary(combined_target_lane, front_vertex, is_left);
+
+    if (dist_to_boundary < margin) {
+      RCLCPP_DEBUG(get_logger(), "Path footprint exceeds target lane boundary");
+      return true;
+    }
+  }
+
+  return false;
+}
+
 std::optional<LaneChangePath> constructCandidatePath(
   const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info,
   const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment,
@@ -1048,6 +1078,14 @@ Polygon2d getEgoCurrentFootprint(
   return autoware::universe_utils::toFootprint(ego_pose, base_to_front, base_to_rear, width);
 }
 
+Point getEgoFrontVertex(
+  const Pose & ego_pose, const autoware::vehicle_info_utils::VehicleInfo & ego_info, bool left)
+{
+  const double lon_offset = ego_info.wheel_base_m + ego_info.front_overhang_m;
+  const double lat_offset = 0.5 * (left ? ego_info.vehicle_width_m : -ego_info.vehicle_width_m);
+  return autoware::universe_utils::calcOffsetPose(ego_pose, lon_offset, lat_offset, 0.0).position;
+}
+
 bool isWithinIntersection(
   const std::shared_ptr<RouteHandler> & route_handler, const lanelet::ConstLanelet & lanelet,
   const Polygon2d & polygon)