From 314d2a6a08f30e9411f31e3c6e503536f0db8462 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Mon, 9 Sep 2024 23:04:48 +0900 Subject: [PATCH] feat(goal_planner): dense goal candidate sampling in BusStopArea (#8795) Signed-off-by: Mamoru Sobue --- .../CMakeLists.txt | 10 +++ .../config/goal_planner.param.yaml | 4 + .../goal_planner_parameters.hpp | 6 ++ .../goal_searcher.hpp | 2 - .../util.hpp | 17 ++++ .../package.xml | 2 + .../src/goal_searcher.cpp | 53 ++++++------ .../src/manager.cpp | 6 ++ .../src/util.cpp | 35 ++++++++ .../test/test_util.cpp | 84 +++++++++++++++++++ 10 files changed, 188 insertions(+), 31 deletions(-) create mode 100644 planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt index 36013c5aa0fb9..626b47b6bdb0d 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt @@ -41,4 +41,14 @@ if(BUILD_EXAMPLES) endforeach() endif() +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + file(GLOB_RECURSE TEST_SOURCES test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} + ${TEST_SOURCES} + ) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + ament_auto_package(INSTALL_TO_SHARE config) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index 379dad254fbec..58a8f5d074fca 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -23,6 +23,10 @@ ignore_distance_from_lane_start: 0.0 margin_from_boundary: 0.75 high_curvature_threshold: 0.1 + bus_stop_area: + use_bus_stop_area: false + goal_search_interval: 0.5 + lateral_offset_interval: 0.25 # occupancy grid map occupancy_grid: diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp index 1272ea38451ee..440e57b53d07f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp @@ -61,6 +61,12 @@ struct GoalPlannerParameters double ignore_distance_from_lane_start{0.0}; double margin_from_boundary{0.0}; double high_curvature_threshold{0.0}; + struct BusStopArea + { + bool use_bus_stop_area{false}; + double goal_search_interval{0.0}; + double lateral_offset_interval{0.0}; + } bus_stop_area; // occupancy grid map bool use_occupancy_grid_for_goal_search{false}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp index dd81650394740..af2e756e64ca5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp @@ -65,8 +65,6 @@ class GoalSearcher : public GoalSearcherBase const std::shared_ptr & planner_data) const; BasicPolygons2d getNoParkingAreaPolygons(const lanelet::ConstLanelets & lanes) const; BasicPolygons2d getNoStoppingAreaPolygons(const lanelet::ConstLanelets & lanes) const; - BasicPolygons2d getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) const; - bool isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const; LinearRing2d vehicle_footprint_{}; bool left_side_parking_{true}; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp index 8fbe2facc87d1..054405b94f617 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp @@ -123,6 +123,23 @@ std::vector createPathFootPrints( const PathWithLaneId & path, const double base_to_front, const double base_to_rear, const double width); +/** + * @brief check if footprint intersects with given areas + */ +bool isIntersectingAreas( + const LinearRing2d & footprint, const std::vector & areas); + +/** + * @brief check if footprint is within one of the areas + */ +bool isWithinAreas( + const LinearRing2d & footprint, const std::vector & areas); + +/** + * @brief query BusStopArea polygons associated with given lanes + */ +std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes); + // debug MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml index 2f107c5644f2e..00d7eb8c9debe 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/package.xml @@ -31,8 +31,10 @@ visualization_msgs ament_cmake_ros + ament_index_cpp ament_lint_auto autoware_lint_common + autoware_test_utils ament_cmake diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp index 54f0b6bfabf87..b9351e4588d5a 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp @@ -19,7 +19,6 @@ #include "autoware/behavior_path_planner_common/utils/path_utils.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" -#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_parking_area.hpp" #include "autoware_lanelet2_extension/regulatory_elements/no_stopping_area.hpp" #include "autoware_lanelet2_extension/utility/query.hpp" @@ -107,7 +106,10 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const double forward_length = parameters_.forward_goal_search_length; const double backward_length = parameters_.backward_goal_search_length; const double margin_from_boundary = parameters_.margin_from_boundary; - const double lateral_offset_interval = parameters_.lateral_offset_interval; + const bool use_bus_stop_area = parameters_.bus_stop_area.use_bus_stop_area; + const double lateral_offset_interval = use_bus_stop_area + ? parameters_.bus_stop_area.lateral_offset_interval + : parameters_.lateral_offset_interval; const double max_lateral_offset = parameters_.max_lateral_offset; const double ignore_distance_from_lane_start = parameters_.ignore_distance_from_lane_start; const double vehicle_width = planner_data->parameters.vehicle_width; @@ -126,9 +128,15 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); const double s_end = goal_arc_coords.length + forward_length; + const double longitudinal_interval = use_bus_stop_area + ? parameters_.bus_stop_area.goal_search_interval + : parameters_.goal_search_interval; auto center_line_path = utils::resamplePathWithSpline( - route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), - parameters_.goal_search_interval); + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), longitudinal_interval); + + const auto no_parking_area_polygons = getNoParkingAreaPolygons(pull_over_lanes); + const auto no_stopping_area_polygons = getNoStoppingAreaPolygons(pull_over_lanes); + const auto bus_stop_area_polygons = goal_planner_utils::getBusStopAreaPolygons(pull_over_lanes); std::vector original_search_poses{}; // for search area visualizing size_t goal_id = 0; @@ -152,7 +160,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const double sign = left_side_parking_ ? -1.0 : 1.0; const double offset_from_center_line = - -distance_from_bound.value() + sign * margin_from_boundary; + use_bus_stop_area ? -distance_from_bound.value() + : -distance_from_bound.value() + sign * margin_from_boundary; // original means non lateral offset poses const Pose original_search_pose = calcOffsetPose(center_pose, 0, offset_from_center_line, 0); const double longitudinal_distance_from_original_goal = @@ -167,12 +176,20 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, autoware::universe_utils::pose2transform(search_pose)); - if (isInAreas(transformed_vehicle_footprint, getNoParkingAreaPolygons(pull_over_lanes))) { + if ( + parameters_.bus_stop_area.use_bus_stop_area && + !goal_planner_utils::isWithinAreas(transformed_vehicle_footprint, bus_stop_area_polygons)) { + continue; + } + + if (goal_planner_utils::isIntersectingAreas( + transformed_vehicle_footprint, no_parking_area_polygons)) { // break here to exclude goals located laterally in no_parking_areas break; } - if (isInAreas(transformed_vehicle_footprint, getNoStoppingAreaPolygons(pull_over_lanes))) { + if (goal_planner_utils::isIntersectingAreas( + transformed_vehicle_footprint, no_stopping_area_polygons)) { // break here to exclude goals located laterally in no_stopping_areas break; } @@ -492,28 +509,6 @@ BasicPolygons2d GoalSearcher::getNoStoppingAreaPolygons(const lanelet::ConstLane return area_polygons; } -BasicPolygons2d GoalSearcher::getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) const -{ - BasicPolygons2d area_polygons{}; - for (const auto & bus_stop_area_reg_elem : lanelet::utils::query::busStopAreas(lanes)) { - for (const auto & area : bus_stop_area_reg_elem->busStopAreas()) { - const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); - area_polygons.push_back(area_poly); - } - } - return area_polygons; -} - -bool GoalSearcher::isInAreas(const LinearRing2d & footprint, const BasicPolygons2d & areas) const -{ - for (const auto & area : areas) { - if (boost::geometry::intersects(area, footprint)) { - return true; - } - } - return false; -} - GoalCandidate GoalSearcher::getClosetGoalCandidateAlongLanes( const GoalCandidates & goal_candidates, const std::shared_ptr & planner_data) const diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp index f9afe28d72d3a..32d6c55276876 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp @@ -59,6 +59,12 @@ GoalPlannerParameters GoalPlannerModuleManager::initGoalPlannerParameters( node->declare_parameter(ns + "ignore_distance_from_lane_start"); p.margin_from_boundary = node->declare_parameter(ns + "margin_from_boundary"); p.high_curvature_threshold = node->declare_parameter(ns + "high_curvature_threshold"); + p.bus_stop_area.use_bus_stop_area = + node->declare_parameter(ns + "bus_stop_area.use_bus_stop_area"); + p.bus_stop_area.goal_search_interval = + node->declare_parameter(ns + "bus_stop_area.goal_search_interval"); + p.bus_stop_area.lateral_offset_interval = + node->declare_parameter(ns + "bus_stop_area.lateral_offset_interval"); const std::string parking_policy_name = node->declare_parameter(ns + "parking_policy"); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 656cdf91c0c54..69873b5e0fb80 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -16,6 +16,7 @@ #include "autoware/behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware/behavior_path_planner_common/utils/utils.hpp" +#include "autoware_lanelet2_extension/regulatory_elements/bus_stop_area.hpp" #include #include @@ -290,6 +291,40 @@ PredictedObjects filterObjectsByLateralDistance( return filtered_objects; } +bool isIntersectingAreas( + const LinearRing2d & footprint, const std::vector & areas) +{ + for (const auto & area : areas) { + if (boost::geometry::intersects(area, footprint)) { + return true; + } + } + return false; +} + +bool isWithinAreas( + const LinearRing2d & footprint, const std::vector & areas) +{ + for (const auto & area : areas) { + if (boost::geometry::within(footprint, area)) { + return true; + } + } + return false; +} + +std::vector getBusStopAreaPolygons(const lanelet::ConstLanelets & lanes) +{ + std::vector area_polygons{}; + for (const auto & bus_stop_area_reg_elem : lanelet::utils::query::busStopAreas(lanes)) { + for (const auto & area : bus_stop_area_reg_elem->busStopAreas()) { + const auto & area_poly = lanelet::utils::to2D(area).basicPolygon(); + area_polygons.push_back(area_poly); + } + } + return area_polygons; +} + MarkerArray createPullOverAreaMarkerArray( const autoware::universe_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, const std_msgs::msg::ColorRGBA & color, const double z) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp new file mode 100644 index 0000000000000..ca93505fe1eae --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -0,0 +1,84 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include + +class TestUtilWithMap : public ::testing::Test +{ +protected: + void SetUp() override + { + rclcpp::init(0, nullptr); + // parameters + auto node_options = rclcpp::NodeOptions{}; + node_options.arguments(std::vector{ + "--ros-args", "--params-file", + ament_index_cpp::get_package_share_directory("autoware_test_utils") + + "/config/test_vehicle_info.param.yaml"}); + auto node = rclcpp::Node::make_shared("test", node_options); + vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(*node).getVehicleInfo(); + + // lanelet map + const std::string shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_behavior_path_planner_common", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + + // load map + route_handler = std::make_shared(map_bin_msg); + } + + void TearDown() override { rclcpp::shutdown(); } + +public: + std::shared_ptr route_handler; + autoware::vehicle_info_utils::VehicleInfo vehicle_info; +}; + +TEST_F(TestUtilWithMap, getBusStopAreaPolygons) +{ + const auto lanes = lanelet::utils::query::laneletLayer(route_handler->getLaneletMapPtr()); + const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes); + const auto bus_stop_area_polygons = + autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes); + EXPECT_EQ(bus_stop_area_polygons.size(), 2); +} + +TEST_F(TestUtilWithMap, isWithinAreas) +{ + const auto lanes = lanelet::utils::query::laneletLayer(route_handler->getLaneletMapPtr()); + const auto shoulder_lanes = lanelet::utils::query::shoulderLanelets(lanes); + const auto bus_stop_area_polygons = + autoware::behavior_path_planner::goal_planner_utils::getBusStopAreaPolygons(shoulder_lanes); + + const auto footprint = vehicle_info.createFootprint(); + const geometry_msgs::msg::Pose baselink_pose = + geometry_msgs::build() + .position( + geometry_msgs::build().x(273.102814).y(402.194794).z(0.0)) + .orientation( + geometry_msgs::build().x(0.0).y(0.0).z(0.707390).w( + 0.706824)); + const auto baselink_footprint = autoware::universe_utils::transformVector( + footprint, autoware::universe_utils::pose2transform(baselink_pose)); + EXPECT_EQ( + autoware::behavior_path_planner::goal_planner_utils::isWithinAreas( + baselink_footprint, bus_stop_area_polygons), + true); +}