From c1011857f1f31ccdff9ede41b51f004889c51786 Mon Sep 17 00:00:00 2001 From: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> Date: Thu, 5 Sep 2024 11:56:43 +0900 Subject: [PATCH] fix(mission_planner): improve condition to check if the goal is within the lane (#1506) fix(mission_planner): improve condition to check if the goal is within the lane (#8710) Signed-off-by: Maxime CLEMENT --- .../src/lanelet2_plugins/default_planner.cpp | 108 +++++++++--------- .../src/lanelet2_plugins/default_planner.hpp | 22 ++-- .../lanelet2_plugins/utility_functions.cpp | 60 ---------- .../lanelet2_plugins/utility_functions.hpp | 12 -- 4 files changed, 65 insertions(+), 137 deletions(-) diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp index 69c52b9c2eafb..2097c0576c545 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -17,6 +17,7 @@ #include "utility_functions.hpp" #include +#include #include #include #include @@ -27,10 +28,13 @@ #include #include +#include +#include + +#include #include +#include #include -#include -#include #include #include @@ -216,7 +220,7 @@ PlannerPlugin::MarkerArray DefaultPlanner::visualize(const LaneletRoute & route) } visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint) const + autoware::universe_utils::LinearRing2d goal_footprint) { visualization_msgs::msg::MarkerArray msg; auto marker = autoware::universe_utils::createDefaultMarker( @@ -244,52 +248,58 @@ visualization_msgs::msg::MarkerArray DefaultPlanner::visualize_debug_footprint( return msg; } -bool DefaultPlanner::check_goal_footprint_inside_lanes( - const lanelet::ConstLanelet & current_lanelet, - const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, - const double search_margin) +lanelet::ConstLanelets next_lanelets_up_to( + const lanelet::ConstLanelet & start_lanelet, const double up_to_distance, + const route_handler::RouteHandler & route_handler) { - // check if goal footprint is in current lane - if (boost::geometry::within(goal_footprint, combined_prev_lanelet.polygon2d().basicPolygon())) { - return true; + lanelet::ConstLanelets lanelets; + if (up_to_distance <= 0.0) { + return lanelets; } - const auto following = route_handler_.getNextLanelets(current_lanelet); - // check if goal footprint is in between many lanelets in depth-first search manner - for (const auto & next_lane : following) { - next_lane_length += lanelet::utils::getLaneletLength2d(next_lane); - lanelet::ConstLanelets lanelets; - lanelets.push_back(combined_prev_lanelet); + for (const auto & next_lane : route_handler.getNextLanelets(start_lanelet)) { lanelets.push_back(next_lane); - lanelet::ConstLanelet combined_lanelets = - combine_lanelets_with_shoulder(lanelets, route_handler_); - - // if next lanelet length is longer than vehicle longitudinal offset - if (vehicle_info_.max_longitudinal_offset_m + search_margin < next_lane_length) { - next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); - // and if the goal_footprint is within the (accumulated) combined_lanelets, terminate the - // query - if (boost::geometry::within(goal_footprint, combined_lanelets.polygon2d().basicPolygon())) { - return true; - } - // if not, iteration continues to next next_lane, and this subtree is terminated - } else { // if next lanelet length is shorter than vehicle longitudinal offset, check the - // overlap with the polygon including the next_lane(s) until the additional lanes get - // longer than ego vehicle length - if (!check_goal_footprint_inside_lanes( - next_lane, combined_lanelets, goal_footprint, next_lane_length)) { - next_lane_length -= lanelet::utils::getLaneletLength2d(next_lane); - continue; - } else { - return true; - } + const auto next_lanelets = next_lanelets_up_to( + next_lane, up_to_distance - lanelet::geometry::length2d(next_lane), route_handler); + lanelets.insert(lanelets.end(), next_lanelets.begin(), next_lanelets.end()); + } + return lanelets; +} + +bool DefaultPlanner::check_goal_footprint_inside_lanes( + const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets, + const universe_utils::Polygon2d & goal_footprint) const +{ + universe_utils::MultiPolygon2d ego_lanes; + universe_utils::Polygon2d poly; + for (const auto & ll : path_lanelets) { + const auto left_shoulder = route_handler_.getLeftShoulderLanelet(ll); + if (left_shoulder) { + boost::geometry::convert(left_shoulder->polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); + } + const auto right_shoulder = route_handler_.getRightShoulderLanelet(ll); + if (right_shoulder) { + boost::geometry::convert(right_shoulder->polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); } + boost::geometry::convert(ll.polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); } - return false; + const auto next_lanelets = + next_lanelets_up_to(current_lanelet, vehicle_info_.max_longitudinal_offset_m, route_handler_); + for (const auto & ll : next_lanelets) { + boost::geometry::convert(ll.polygon2d().basicPolygon(), poly); + ego_lanes.push_back(poly); + } + + // check if goal footprint is in the ego lane + universe_utils::MultiPolygon2d difference; + boost::geometry::difference(goal_footprint, ego_lanes, difference); + return boost::geometry::is_empty(difference); } bool DefaultPlanner::is_goal_valid( - const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets) { const auto logger = node_->get_logger(); @@ -337,16 +347,10 @@ bool DefaultPlanner::is_goal_valid( pub_goal_footprint_marker_->publish(visualize_debug_footprint(goal_footprint)); const auto polygon_footprint = convert_linear_ring_to_polygon(goal_footprint); - double next_lane_length = 0.0; - // combine calculated route lanelets - const lanelet::ConstLanelet combined_prev_lanelet = - combine_lanelets_with_shoulder(path_lanelets, route_handler_); - // check if goal footprint exceeds lane when the goal isn't in parking_lot if ( param_.check_footprint_inside_lanes && - !check_goal_footprint_inside_lanes( - closest_lanelet, combined_prev_lanelet, polygon_footprint, next_lane_length) && + !check_goal_footprint_inside_lanes(closest_lanelet, path_lanelets, polygon_footprint) && !is_in_parking_lot( lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()), lanelet::utils::conversion::toLaneletPoint(goal.position))) { @@ -375,11 +379,7 @@ bool DefaultPlanner::is_goal_valid( // check if goal is in parking lot const auto parking_lots = lanelet::utils::query::getAllParkingLots(route_handler_.getLaneletMapPtr()); - if (is_in_parking_lot(parking_lots, goal_lanelet_pt)) { - return true; - } - - return false; + return is_in_parking_lot(parking_lots, goal_lanelet_pt); } PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) @@ -429,7 +429,7 @@ PlannerPlugin::LaneletRoute DefaultPlanner::plan(const RoutePoints & points) return route_msg; } - if (route_handler_.isRouteLooped(route_sections)) { + if (route_handler::RouteHandler::isRouteLooped(route_sections)) { RCLCPP_WARN(logger, "Loop detected within route!"); return route_msg; } diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp index 60c2a53b82123..39b7fa8909a6a 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/default_planner.hpp @@ -27,7 +27,6 @@ #include #include -#include #include namespace autoware::mission_planner::lanelet2 @@ -44,15 +43,17 @@ struct DefaultPlannerParameters class DefaultPlanner : public mission_planner::PlannerPlugin { public: + DefaultPlanner() : vehicle_info_(), is_graph_ready_(false), param_(), node_(nullptr) {} + void initialize(rclcpp::Node * node) override; void initialize(rclcpp::Node * node, const LaneletMapBin::ConstSharedPtr msg) override; - bool ready() const override; + [[nodiscard]] bool ready() const override; LaneletRoute plan(const RoutePoints & points) override; void updateRoute(const PlannerPlugin::LaneletRoute & route) override; void clearRoute() override; - MarkerArray visualize(const LaneletRoute & route) const override; - MarkerArray visualize_debug_footprint( - autoware::universe_utils::LinearRing2d goal_footprint_) const; + [[nodiscard]] MarkerArray visualize(const LaneletRoute & route) const override; + [[nodiscard]] static MarkerArray visualize_debug_footprint( + autoware::universe_utils::LinearRing2d goal_footprint); autoware::vehicle_info_utils::VehicleInfo vehicle_info_; private: @@ -83,17 +84,16 @@ class DefaultPlanner : public mission_planner::PlannerPlugin * @param next_lane_length the accumulated total length from the start lanelet of the search to * the lanelet of current goal query */ - bool check_goal_footprint_inside_lanes( - const lanelet::ConstLanelet & current_lanelet, - const lanelet::ConstLanelet & combined_prev_lanelet, - const autoware::universe_utils::Polygon2d & goal_footprint, double & next_lane_length, - const double search_margin = 2.0); + [[nodiscard]] bool check_goal_footprint_inside_lanes( + const lanelet::ConstLanelet & current_lanelet, const lanelet::ConstLanelets & path_lanelets, + const universe_utils::Polygon2d & goal_footprint) const; /** * @brief return true if (1)the goal is in parking area or (2)the goal is on the lanes and the * footprint around the goal does not overlap the lanes */ - bool is_goal_valid(const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets); + bool is_goal_valid( + const geometry_msgs::msg::Pose & goal, const lanelet::ConstLanelets & path_lanelets); /** * @brief project the specified goal pose onto the goal lanelet(the last preferred lanelet of diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp index 5046b81c72b96..34b16ef35f603 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -18,9 +18,6 @@ #include -#include -#include - autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( autoware::universe_utils::LinearRing2d footprint) { @@ -41,63 +38,6 @@ void insert_marker_array( a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); } -lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, - const autoware::route_handler::RouteHandler & route_handler) -{ - lanelet::Points3d lefts; - lanelet::Points3d rights; - lanelet::Points3d centers; - std::vector left_bound_ids; - std::vector right_bound_ids; - - for (const auto & llt : lanelets) { - if (llt.id() != lanelet::InvalId) { - left_bound_ids.push_back(llt.leftBound().id()); - right_bound_ids.push_back(llt.rightBound().id()); - } - } - - // lambda to add bound to target_bound - const auto add_bound = [](const auto & bound, auto & target_bound) { - std::transform( - bound.begin(), bound.end(), std::back_inserter(target_bound), - [](const auto & pt) { return lanelet::Point3d(pt); }); - }; - for (const auto & llt : lanelets) { - // check if shoulder lanelets which has RIGHT bound same to LEFT bound of lanelet exist - const auto left_shared_shoulder = route_handler.getLeftShoulderLanelet(llt); - if (left_shared_shoulder) { - // if exist, add left bound of SHOULDER lanelet to lefts - add_bound(left_shared_shoulder->leftBound(), lefts); - } else if ( - // if not exist, add left bound of lanelet to lefts - // if the **left** of this lanelet does not match any of the **right** bounds of `lanelets`, - // then its left bound constitutes the left boundary of the entire merged lanelet - std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { - add_bound(llt.leftBound(), lefts); - } - - // check if shoulder lanelets which has LEFT bound same to RIGHT bound of lanelet exist - const auto right_shared_shoulder = route_handler.getRightShoulderLanelet(llt); - if (right_shared_shoulder) { - // if exist, add right bound of SHOULDER lanelet to rights - add_bound(right_shared_shoulder->rightBound(), rights); - } else if ( - // if not exist, add right bound of lanelet to rights - // if the **right** of this lanelet does not match any of the **left** bounds of `lanelets`, - // then its right bound constitutes the right boundary of the entire merged lanelet - std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { - add_bound(llt.rightBound(), rights); - } - } - - const auto left_bound = lanelet::LineString3d(lanelet::InvalId, lefts); - const auto right_bound = lanelet::LineString3d(lanelet::InvalId, rights); - auto combined_lanelet = lanelet::Lanelet(lanelet::InvalId, left_bound, right_bound); - return std::move(combined_lanelet); -} - std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet) { std::vector centerline_points; diff --git a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp index c8812e2c76dd6..6e3d2a0e88941 100644 --- a/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp +++ b/planning/autoware_mission_planner/src/lanelet2_plugins/utility_functions.hpp @@ -27,8 +27,6 @@ #include #include -#include -#include #include template @@ -47,16 +45,6 @@ autoware::universe_utils::Polygon2d convert_linear_ring_to_polygon( void insert_marker_array( visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2); -/** - * @brief create a single merged lanelet whose left/right bounds consist of the leftmost/rightmost - * bound of the original lanelets or the left/right bound of its adjacent road_shoulder respectively - * @param lanelets topologically sorted sequence of lanelets - * @param route_handler route handler to query the lanelet map - */ -lanelet::ConstLanelet combine_lanelets_with_shoulder( - const lanelet::ConstLanelets & lanelets, - const autoware::route_handler::RouteHandler & route_handler); - std::vector convertCenterlineToPoints(const lanelet::Lanelet & lanelet); geometry_msgs::msg::Pose convertBasicPoint3dToPose( const lanelet::BasicPoint3d & point, const double lane_yaw);