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 4c55beb89e813..0a2c9991ec5fd 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 @@ -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/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 194412dfe01b7..4ba2e1aef893c 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 @@ -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/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 84c3f310f0382..bf80b492d533e 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 @@ -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; @@ -94,6 +95,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 constructCandidatePath( const CommonDataPtr & common_data_ptr, const LaneChangeInfo & lane_change_info, const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, @@ -217,8 +222,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. @@ -305,8 +311,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 index 69df2fe14317a..dd7382d8f7116 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 @@ -102,6 +102,8 @@ void LaneChangeModuleManager::initParams(rclcpp::Node * node) // safety check p.allow_loose_check_for_cancel = getOrDeclareParameter(*node, parameter("safety_check.allow_loose_check_for_cancel")); + p.enable_target_lane_bound_check = + getOrDeclareParameter(*node, parameter("safety_check.enable_target_lane_bound_check")); p.collision_check_yaw_diff_threshold = getOrDeclareParameter( *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 index e59119230c224..61f84c31c88da 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 @@ -1686,6 +1686,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 index 6ff69abb85d18..d163c00369b12 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 @@ -212,6 +212,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 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 & route_handler, const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon)