diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp index 04bcfbacbbc17..d986d0f23fc85 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_geometry.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #define EIGEN_MPL2_ONLY #include @@ -98,5 +99,6 @@ BOOST_GEOMETRY_REGISTER_POINT_2D( // NOLIN tier4_autoware_utils::Point2d, double, cs::cartesian, x(), y()) // NOLINT BOOST_GEOMETRY_REGISTER_POINT_3D( // NOLINT tier4_autoware_utils::Point3d, double, cs::cartesian, x(), y(), z()) // NOLINT +BOOST_GEOMETRY_REGISTER_RING(tier4_autoware_utils::LinearRing2d) // NOLINT #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_GEOMETRY_HPP_ diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index dc55576640133..c658cf4497973 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -29,14 +29,20 @@ #include #include +#include +#include #include -#include #include +#include +#include +#include +#include #include #include #include +#include #include namespace lane_departure_checker @@ -112,6 +118,19 @@ class LaneDepartureChecker bool checkPathWillLeaveLane( const lanelet::ConstLanelets & lanelets, const PathWithLaneId & path) const; + std::vector> getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + std::optional getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + bool checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const; + + PathWithLaneId cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, + const size_t end_index); + static bool isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 5bd0fcace9909..c48383a17ab4b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -34,6 +34,7 @@ using motion_utils::calcArcLength; using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::MultiPoint2d; +using tier4_autoware_utils::MultiPolygon2d; using tier4_autoware_utils::Point2d; namespace @@ -92,6 +93,7 @@ lanelet::ConstLanelets getCandidateLanelets( return candidate_lanelets; } + } // namespace namespace lane_departure_checker @@ -298,6 +300,98 @@ bool LaneDepartureChecker::willLeaveLane( return false; } +std::vector> LaneDepartureChecker::getLaneletsFromPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // Get Footprint Hull basic polygon + std::vector vehicle_footprints = createVehicleFootprints(path); + LinearRing2d footprint_hull = createHullFromFootprints(vehicle_footprints); + auto to_basic_polygon = [](const LinearRing2d & footprint_hull) -> lanelet::BasicPolygon2d { + lanelet::BasicPolygon2d basic_polygon; + for (const auto & point : footprint_hull) { + Eigen::Vector2d p(point.x(), point.y()); + basic_polygon.push_back(p); + } + return basic_polygon; + }; + lanelet::BasicPolygon2d footprint_hull_basic_polygon = to_basic_polygon(footprint_hull); + + // Find all lanelets that intersect the footprint hull + return lanelet::geometry::findWithin2d( + lanelet_map_ptr->laneletLayer, footprint_hull_basic_polygon, 0.0); +} + +std::optional LaneDepartureChecker::getFusedLaneletPolygonForPath( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + const auto lanelets_distance_pair = getLaneletsFromPath(lanelet_map_ptr, path); + auto to_polygon2d = [](const lanelet::BasicPolygon2d & poly) -> tier4_autoware_utils::Polygon2d { + tier4_autoware_utils::Polygon2d p; + auto & outer = p.outer(); + + for (const auto & p : poly) { + tier4_autoware_utils::Point2d p2d(p.x(), p.y()); + outer.push_back(p2d); + } + boost::geometry::correct(p); + return p; + }; + + // Fuse lanelets into a single BasicPolygon2d + auto fused_lanelets = [&]() -> std::optional { + if (lanelets_distance_pair.empty()) return std::nullopt; + tier4_autoware_utils::MultiPolygon2d lanelet_unions; + tier4_autoware_utils::MultiPolygon2d result; + + for (size_t i = 0; i < lanelets_distance_pair.size(); ++i) { + const auto & route_lanelet = lanelets_distance_pair.at(i).second; + const auto & p = route_lanelet.polygon2d().basicPolygon(); + tier4_autoware_utils::Polygon2d poly = to_polygon2d(p); + boost::geometry::union_(lanelet_unions, poly, result); + lanelet_unions = result; + result.clear(); + } + + if (lanelet_unions.empty()) return std::nullopt; + return lanelet_unions.front(); + }(); + + return fused_lanelets; +} + +bool LaneDepartureChecker::checkPathWillLeaveLane( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path) const +{ + // check if the footprint is not fully contained within the fused lanelets polygon + const std::vector vehicle_footprints = createVehicleFootprints(path); + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (!fused_lanelets_polygon) return true; + return !std::all_of( + vehicle_footprints.begin(), vehicle_footprints.end(), + [&fused_lanelets_polygon](const auto & footprint) { + return boost::geometry::within(footprint, fused_lanelets_polygon.value()); + }); +} + +PathWithLaneId LaneDepartureChecker::cropPointsOutsideOfLanes( + const lanelet::LaneletMapPtr lanelet_map_ptr, const PathWithLaneId & path, const size_t end_index) +{ + PathWithLaneId temp_path; + const auto fused_lanelets_polygon = getFusedLaneletPolygonForPath(lanelet_map_ptr, path); + if (path.points.empty() || !fused_lanelets_polygon) return temp_path; + const auto vehicle_footprints = createVehicleFootprints(path); + size_t idx = 0; + std::for_each(vehicle_footprints.begin(), vehicle_footprints.end(), [&](const auto & footprint) { + if (idx > end_index || boost::geometry::within(footprint, fused_lanelets_polygon.value())) { + temp_path.points.push_back(path.points.at(idx)); + } + ++idx; + }); + PathWithLaneId cropped_path = path; + cropped_path.points = temp_path.points; + return cropped_path; +} + bool LaneDepartureChecker::isOutOfLane( const lanelet::ConstLanelets & candidate_lanelets, const LinearRing2d & vehicle_footprint) { @@ -364,4 +458,5 @@ bool LaneDepartureChecker::willCrossBoundary( } return false; } + } // namespace lane_departure_checker diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp index 4d1f1fdb959c0..3a2465eda442c 100644 --- a/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp +++ b/planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp @@ -18,6 +18,7 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" +#include #include #include @@ -78,7 +79,8 @@ class GeometricParallelParking const bool left_side_parking); bool planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start); + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const std::shared_ptr lane_departure_checker); void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; } void setPlannerData(const std::shared_ptr & planner_data) { @@ -121,7 +123,8 @@ class GeometricParallelParking const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double lane_departure_margin, const double arc_path_interval); + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr lane_departure_checker); PathWithLaneId generateArcPath( const Pose & center, const double radius, const double start_yaw, double end_yaw, const double arc_path_interval, const bool is_left_turn, const bool is_forward); diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index b289ce86ca60e..3dba32cec2399 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -47,6 +47,7 @@ freespace_planning_algorithms geometry_msgs interpolation + lane_departure_checker lanelet2_extension libboost-dev libopencv-dev diff --git a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp index 7ba4f114328a7..8a89aa139ae44 100644 --- a/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp @@ -132,7 +132,7 @@ std::vector GeometricParallelParking::generatePullOverPaths( : parameters_.backward_parking_path_interval; auto arc_paths = planOneTrial( start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking, - end_pose_offset, lane_departure_margin, arc_path_interval); + end_pose_offset, lane_departure_margin, arc_path_interval, {}); if (arc_paths.empty()) { return std::vector{}; } @@ -238,7 +238,8 @@ bool GeometricParallelParking::planPullOver( bool GeometricParallelParking::planPullOut( const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes, - const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start) + const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start, + const std::shared_ptr lane_departure_checker) { constexpr bool is_forward = false; // parking backward means pull_out forward constexpr double start_pose_offset = 0.0; // start_pose is current_pose @@ -258,7 +259,7 @@ bool GeometricParallelParking::planPullOut( auto arc_paths = planOneTrial( *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start, start_pose_offset, parameters_.pull_out_lane_departure_margin, - parameters_.pull_out_path_interval); + parameters_.pull_out_path_interval, lane_departure_checker); if (arc_paths.empty()) { // not found path continue; @@ -378,7 +379,8 @@ std::vector GeometricParallelParking::planOneTrial( const Pose & start_pose, const Pose & goal_pose, const double R_E_far, const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward, const bool left_side_parking, const double end_pose_offset, - const double lane_departure_margin, const double arc_path_interval) + const double lane_departure_margin, const double arc_path_interval, + const std::shared_ptr lane_departure_checker) { clearPaths(); @@ -512,6 +514,24 @@ std::vector GeometricParallelParking::planOneTrial( setLaneIdsToPath(path_turn_first); setLaneIdsToPath(path_turn_second); + if (lane_departure_checker) { + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + + const bool is_path_turn_first_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_first); + + if (is_path_turn_first_outside_lanes) { + return std::vector{}; + } + + const bool is_path_turn_second_outside_lanes = + lane_departure_checker->checkPathWillLeaveLane(lanelet_map_ptr, path_turn_second); + + if (is_path_turn_second_outside_lanes) { + return std::vector{}; + } + } + // generate arc path vector paths_.push_back(path_turn_first); paths_.push_back(path_turn_second); diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp index 164868b47535e..68a5d8100a271 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/geometric_pull_out.hpp @@ -19,20 +19,27 @@ #include "behavior_path_start_planner_module/pull_out_path.hpp" #include "behavior_path_start_planner_module/pull_out_planner_base.hpp" +#include + #include +#include + namespace behavior_path_planner { class GeometricPullOut : public PullOutPlannerBase { public: - explicit GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters); + explicit GeometricPullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const std::shared_ptr lane_departure_checker); PlannerType getPlannerType() override { return PlannerType::GEOMETRIC; }; std::optional plan(const Pose & start_pose, const Pose & goal_pose) override; GeometricParallelParking planner_; ParallelParkingParameters parallel_parking_parameters_; + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp index bf5a64b4d0973..8f7632dee6ca1 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp @@ -244,6 +244,7 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; + lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. bool isGoalBehindOfEgoInSameRouteSegment() const; diff --git a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp index 7d6dd60398e8e..3ff7e998cdff7 100644 --- a/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/geometric_pull_out.cpp @@ -30,9 +30,12 @@ namespace behavior_path_planner { using start_planner_utils::getPullOutLanes; -GeometricPullOut::GeometricPullOut(rclcpp::Node & node, const StartPlannerParameters & parameters) +GeometricPullOut::GeometricPullOut( + rclcpp::Node & node, const StartPlannerParameters & parameters, + const std::shared_ptr lane_departure_checker) : PullOutPlannerBase{node, parameters}, - parallel_parking_parameters_{parameters.parallel_parking_parameters} + parallel_parking_parameters_{parameters.parallel_parking_parameters}, + lane_departure_checker_(lane_departure_checker) { planner_.setParameters(parallel_parking_parameters_); } @@ -55,8 +58,8 @@ std::optional GeometricPullOut::plan(const Pose & start_pose, const planner_.setTurningRadius( planner_data_->parameters, parallel_parking_parameters_.pull_out_max_steer_angle); planner_.setPlannerData(planner_data_); - const bool found_valid_path = - planner_.planPullOut(start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start); + const bool found_valid_path = planner_.planPullOut( + start_pose, goal_pose, road_lanes, pull_out_lanes, left_side_start, lane_departure_checker_); if (!found_valid_path) { return {}; } diff --git a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp index 73952ca02f558..fe83ca45a8185 100644 --- a/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp +++ b/planning/behavior_path_start_planner_module/src/shift_pull_out.cpp @@ -73,11 +73,10 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos // get safe path for (auto & pull_out_path : pull_out_paths) { - auto & shift_path = - pull_out_path.partial_paths.front(); // shift path is not separate but only one. - - // check lane_departure and collision with path between pull_out_start to pull_out_end - PathWithLaneId path_start_to_end{}; + // shift path is not separate but only one. + auto & shift_path = pull_out_path.partial_paths.front(); + // check lane_departure with path between pull_out_start to pull_out_end + PathWithLaneId path_shift_start_to_end{}; { const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); @@ -93,63 +92,35 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos return shift_path.points.size() - 1; } }); - path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + path_shift_start_to_end.points.insert( + path_shift_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, shift_path.points.begin() + collision_check_end_idx + 1); } - // extract shoulder lanes from pull out lanes - lanelet::ConstLanelets shoulder_lanes; - std::copy_if( - pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), - [&route_handler](const auto & pull_out_lane) { - return route_handler->isShoulderLanelet(pull_out_lane); - }); - const auto drivable_lanes = - utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto & dp = planner_data_->drivable_area_expansion_parameters; - const auto expanded_lanes = utils::transformToLanelets(utils::expandLanelets( - drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset, - dp.drivable_area_types_to_skip)); + // check lane departure + // The method for lane departure checking verifies if the footprint of each point on the path is + // contained within a lanelet using `boost::geometry::within`, which incurs a high computational + // cost. + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); + if ( + parameters_.check_shift_path_lane_departure && + lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) { + continue; + } // crop backward path // removes points which are out of lanes up to the start pose. // this ensures that the backward_path stays within the drivable area when starting from a // narrow place. + const size_t start_segment_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( shift_path.points, start_pose, common_parameters.ego_nearest_dist_threshold, common_parameters.ego_nearest_yaw_threshold); - PathWithLaneId cropped_path{}; - for (size_t i = 0; i < shift_path.points.size(); ++i) { - const Pose pose = shift_path.points.at(i).point.pose; - const auto transformed_vehicle_footprint = - transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(pose)); - const bool is_out_of_lane = - LaneDepartureChecker::isOutOfLane(expanded_lanes, transformed_vehicle_footprint); - if (i <= start_segment_idx) { - if (!is_out_of_lane) { - cropped_path.points.push_back(shift_path.points.at(i)); - } - } else { - cropped_path.points.push_back(shift_path.points.at(i)); - } - } - shift_path.points = cropped_path.points; - - // check lane departure - if ( - parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane(expanded_lanes, path_start_to_end)) { - continue; - } - - // check collision - if (utils::checkCollisionBetweenPathFootprintsAndObjects( - vehicle_footprint_, path_start_to_end, pull_out_lane_stop_objects, - parameters_.collision_check_margin)) { - continue; - } + const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( + lanelet_map_ptr, shift_path, start_segment_idx); + if (cropped_path.points.empty()) continue; + shift_path.points = cropped_path.points; shift_path.header = planner_data_->route_handler->getRouteHeader(); return pull_out_path; diff --git a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp index 1eb5aa9191a5c..5ac4b6b141b1a 100644 --- a/planning/behavior_path_start_planner_module/src/start_planner_module.cpp +++ b/planning/behavior_path_start_planner_module/src/start_planner_module.cpp @@ -66,7 +66,8 @@ StartPlannerModule::StartPlannerModule( std::make_shared(node, *parameters, lane_departure_checker_)); } if (parameters_->enable_geometric_pull_out) { - start_planners_.push_back(std::make_shared(node, *parameters)); + start_planners_.push_back( + std::make_shared(node, *parameters, lane_departure_checker_)); } if (start_planners_.empty()) { RCLCPP_ERROR(getLogger(), "Not found enabled planner"); @@ -1289,6 +1290,30 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } +lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const +{ + const double backward_path_length = + planner_data_->parameters.backward_path_length + parameters_->max_back_distance; + const auto pull_out_lanes = + start_planner_utils::getPullOutLanes(planner_data_, backward_path_length); + if (pull_out_lanes.empty()) { + return lanelet::ConstLanelets{}; + } + const auto road_lanes = utils::getExtendedCurrentLanes( + planner_data_, backward_path_length, std::numeric_limits::max(), + /*forward_only_in_route*/ true); + // extract shoulder lanes from pull out lanes + lanelet::ConstLanelets shoulder_lanes; + std::copy_if( + pull_out_lanes.begin(), pull_out_lanes.end(), std::back_inserter(shoulder_lanes), + [this](const auto & pull_out_lane) { + return planner_data_->route_handler->isShoulderLanelet(pull_out_lane); + }); + const auto departure_check_lanes = utils::transformToLanelets( + utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes)); + return departure_check_lanes; +} + void StartPlannerModule::setDebugData() const { using marker_utils::createObjectsMarkerArray;