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..be2411cd3268b 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..811e1652fcb4a 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,92 @@ 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); + // Fuse lanelets into a single BasicPolygon2d + auto fused_lanelets = [&lanelets_distance_pair]() -> std::optional { + if (lanelets_distance_pair.empty()) return std::nullopt; + if (lanelets_distance_pair.size() == 1) + return lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + + lanelet::BasicPolygon2d merged_polygon = + lanelets_distance_pair.at(0).second.polygon2d().basicPolygon(); + for (size_t i = 1; i < lanelets_distance_pair.size(); ++i) { + const auto & route_lanelet = lanelets_distance_pair.at(i).second; + const auto & poly = route_lanelet.polygon2d().basicPolygon(); + + std::vector lanelet_union_temp; + boost::geometry::union_(poly, merged_polygon, lanelet_union_temp); + + // Update merged_polygon by accumulating all merged results + merged_polygon.clear(); + for (const auto & temp_poly : lanelet_union_temp) { + merged_polygon.insert(merged_polygon.end(), temp_poly.begin(), temp_poly.end()); + } + } + if (merged_polygon.empty()) return std::nullopt; + return merged_polygon; + }(); + + 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 +452,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 26c0f247eb558..1489dd1beff07 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,8 @@ #include "behavior_path_planner_common/data_manager.hpp" #include "behavior_path_planner_common/parameters.hpp" +#include + #include #include #include @@ -72,7 +74,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) { @@ -115,7 +118,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 db5dc51150e55..2dd4d12c05487 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -49,6 +49,7 @@ freespace_planning_algorithms geometry_msgs interpolation + lane_departure_checker lanelet2_extension magic_enum motion_utils 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 a1a079e679e72..9400abd20b984 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 @@ -116,7 +116,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{}; } @@ -222,7 +222,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 @@ -242,7 +243,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; @@ -362,7 +363,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(); @@ -496,6 +498,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 e2b91ce3789be..97217060d3c20 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 @@ -273,6 +273,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 d9ff155f36e6c..a5aeb5b257d39 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 36b8b1dc347a1..1d86018c424dd 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 @@ -65,9 +65,8 @@ 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. - + // 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{}; { @@ -80,51 +79,30 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_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_shift_start_to_end)) { - continue; - } + const auto cropped_path = lane_departure_checker_->cropPointsOutsideOfLanes( + lanelet_map_ptr, shift_path, start_segment_idx); + 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 b4e757e6c0723..83d2b5cd2dab1 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");