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/shift_pull_out.hpp b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp index 6bf85a4679dfd..2d50ae189dc13 100644 --- a/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp +++ b/planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/shift_pull_out.hpp @@ -50,11 +50,6 @@ class ShiftPullOut : public PullOutPlannerBase ShiftedPath & shifted_path, const Pose & start_pose, const Pose & end_pose, const double longitudinal_acc, const double lateral_acc); - void setDepartureCheckLanes(const lanelet::ConstLanelets & departure_check_lanes) - { - departure_check_lanes_ = departure_check_lanes; - } - std::shared_ptr lane_departure_checker_; private: @@ -63,8 +58,6 @@ class ShiftPullOut : public PullOutPlannerBase double calcPullOutLongitudinalDistance( const double lon_acc, const double shift_time, const double shift_length, const double max_curvature, const double min_distance) const; - - lanelet::ConstLanelets departure_check_lanes_; }; } // 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 067bbd5d74aee..aaf711b855e7a 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 @@ -275,7 +275,6 @@ class StartPlannerModule : public SceneModuleInterface const std::vector & ego_predicted_path) const; bool isSafePath() const; void setDrivableAreaInfo(BehaviorModuleOutput & output) const; - void updateDepartureCheckLanes(); lanelet::ConstLanelets createDepartureCheckLanes() const; // check if the goal is located behind the ego in the same route segment. 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 8eca8479fbd44..d21a68048e34f 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 @@ -60,9 +60,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{}; { @@ -75,44 +74,30 @@ std::optional ShiftPullOut::plan(const Pose & start_pose, const Pos shift_path.points.begin() + pull_out_end_idx + 1); } - // 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(departure_check_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 // 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. - // TODO(someone): improve the method for detecting lane departures without using - // lanelet::ConstLanelets, making it unnecessary to retain departure_check_lanes_ as a member - // variable. + const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr(); if ( parameters_.check_shift_path_lane_departure && - lane_departure_checker_->checkPathWillLeaveLane( - departure_check_lanes_, path_shift_start_to_end)) { + 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); + + 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 2171373cd85ff..ed81f39135181 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 @@ -69,7 +69,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"); @@ -142,7 +143,6 @@ void StartPlannerModule::initVariables() info_marker_.markers.clear(); initializeSafetyCheckParameters(); initializeCollisionCheckDebugMap(debug_data_.collision_check); - updateDepartureCheckLanes(); } void StartPlannerModule::updateEgoPredictedPathParams( @@ -608,7 +608,6 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval() void StartPlannerModule::resetStatus() { status_ = PullOutStatus{}; - updateDepartureCheckLanes(); } void StartPlannerModule::incrementPathIndex() @@ -1409,22 +1408,6 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons } } -void StartPlannerModule::updateDepartureCheckLanes() -{ - const auto departure_check_lanes = createDepartureCheckLanes(); - for (auto & planner : start_planners_) { - auto shift_pull_out = std::dynamic_pointer_cast(planner); - - if (shift_pull_out) { - shift_pull_out->setDepartureCheckLanes(departure_check_lanes); - } - } - // debug - { - debug_data_.departure_check_lanes = departure_check_lanes; - } -} - lanelet::ConstLanelets StartPlannerModule::createDepartureCheckLanes() const { const double backward_path_length =