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 ed8b518904d46..0f9eda8efe25c 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 @@ -141,6 +141,29 @@ MarkerArray createLaneletPolygonMarkerArray( MarkerArray createNumObjectsToAvoidTextsMarkerArray( const GoalCandidates & goal_candidates, std::string && ns, const std_msgs::msg::ColorRGBA & color); +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_rough_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature); +/** + * @brief combine two points + * @param points lane points + * @param points_next next lane points + * @return combined points + */ +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next); +/** @brief Create a lanelet that represents the departure check area. + * @param [in] pull_over_lanes Lanelets that the vehicle will pull over to. + * @param [in] route_handler RouteHandler object. + * @return Lanelet that goal footprints should be inside. + */ +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking); } // namespace autoware::behavior_path_planner::goal_planner_utils #endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_ 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 0a135562ddc6b..4f0a18e01d034 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 @@ -115,11 +115,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( *route_handler, left_side_parking_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length); - auto lanes = utils::getExtendedCurrentLanes( - planner_data, backward_length, forward_length, - /*forward_only_in_route*/ false); - lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); - + const auto departure_check_lane = goal_planner_utils::createDepartureCheckLanelet( + pull_over_lanes, *route_handler, left_side_parking_); const auto goal_arc_coords = lanelet::utils::getArcCoordinates(pull_over_lanes, reference_goal_pose_); const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); @@ -177,7 +174,8 @@ GoalCandidates GoalSearcher::search(const std::shared_ptr & p break; } - if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { + if (!boost::geometry::within( + transformed_vehicle_footprint, departure_check_lane.polygon2d().basicPolygon())) { continue; } 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 9ad69411ded49..96cb7d9cdf436 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 @@ -576,4 +576,96 @@ std::vector createPathFootPrints( return footprints; } +std::string makePathPriorityDebugMessage( + const std::vector & sorted_path_indices, + const std::vector & pull_over_path_candidates, + const std::map & goal_id_to_index, const GoalCandidates & goal_candidates, + const std::map & path_id_to_rough_margin_map, + const std::function & isSoftMargin, + const std::function & isHighCurvature) +{ + std::stringstream ss; + + // Unsafe goal and its priority are not visible as debug marker in rviz, + // so exclude unsafe goal from goal_priority + std::map goal_id_and_priority; + for (size_t i = 0; i < goal_candidates.size(); ++i) { + goal_id_and_priority[goal_candidates[i].id] = goal_candidates[i].is_safe ? i : -1; + } + + ss << "\n---------------------- path priority ----------------------\n"; + for (size_t i = 0; i < sorted_path_indices.size(); ++i) { + const auto & path = pull_over_path_candidates[sorted_path_indices[i]]; + // goal_index is same to goal priority including unsafe goal + const int goal_index = static_cast(goal_id_to_index.at(path.goal_id)); + const bool is_safe_goal = goal_candidates[goal_index].is_safe; + const int goal_priority = goal_id_and_priority[path.goal_id]; + + ss << "path_priority: " << i << ", path_type: " << magic_enum::enum_name(path.type) + << ", path_id: " << path.id << ", goal_id: " << path.goal_id + << ", goal_priority: " << (is_safe_goal ? std::to_string(goal_priority) : "unsafe") + << ", margin: " << path_id_to_rough_margin_map.at(path.id) + << (isSoftMargin(path) ? " (soft)" : " (hard)") + << ", curvature: " << path.getParkingPathMaxCurvature() + << (isHighCurvature(path) ? " (high)" : " (low)") << "\n"; + } + ss << "-----------------------------------------------------------\n"; + return ss.str(); +} + +lanelet::Points3d combineLanePoints( + const lanelet::Points3d & points, const lanelet::Points3d & points_next) +{ + lanelet::Points3d combined_points; + std::unordered_set point_ids; + for (const auto & point : points) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + for (const auto & point : points_next) { + if (point_ids.insert(point.id()).second) { + combined_points.push_back(point); + } + } + return combined_points; +} + +lanelet::Lanelet createDepartureCheckLanelet( + const lanelet::ConstLanelets & pull_over_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const auto getBoundPoints = + [&](const lanelet::ConstLanelet & lane, const bool is_outer) -> lanelet::Points3d { + lanelet::Points3d points; + const auto & bound = left_side_parking ? (is_outer ? lane.leftBound() : lane.rightBound()) + : (is_outer ? lane.rightBound() : lane.leftBound()); + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto getMostInnerLane = [&](const lanelet::ConstLanelet & lane) -> lanelet::ConstLanelet { + return left_side_parking ? route_handler.getMostRightLanelet(lane, false, true) + : route_handler.getMostLeftLanelet(lane, false, true); + }; + + lanelet::Points3d outer_bound_points{}; + lanelet::Points3d inner_bound_points{}; + for (const auto & lane : pull_over_lanes) { + const auto current_outer_bound_points = getBoundPoints(lane, true); + const auto most_inner_lane = getMostInnerLane(lane); + const auto current_inner_bound_points = getBoundPoints(most_inner_lane, false); + outer_bound_points = combineLanePoints(outer_bound_points, current_outer_bound_points); + inner_bound_points = combineLanePoints(inner_bound_points, current_inner_bound_points); + } + + const auto outer_linestring = lanelet::LineString3d(lanelet::InvalId, outer_bound_points); + const auto inner_linestring = lanelet::LineString3d(lanelet::InvalId, inner_bound_points); + return lanelet::Lanelet( + lanelet::InvalId, left_side_parking ? outer_linestring : inner_linestring, + left_side_parking ? inner_linestring : outer_linestring); +} + } // namespace autoware::behavior_path_planner::goal_planner_utils 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..b50ee083fddc8 --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/test/test_util.cpp @@ -0,0 +1,167 @@ +// 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); +} + +TEST_F(TestUtilWithMap, combineLanePoints) +{ + // 1) combine points with no duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0), lanelet::Point3d(6, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 6); + } + + // 2) combine points with duplicate IDs + { + lanelet::Points3d points{ + {lanelet::Point3d(1, 0, 0), lanelet::Point3d(2, 0, 0), lanelet::Point3d(3, 0, 0)}}; + lanelet::Points3d points_next{ + {lanelet::Point3d(3, 0, 0), lanelet::Point3d(4, 0, 0), lanelet::Point3d(5, 0, 0)}}; + + const auto combined_points = + autoware::behavior_path_planner::goal_planner_utils::combineLanePoints(points, points_next); + EXPECT_EQ(combined_points.size(), 5); + } +} + +TEST_F(TestUtilWithMap, createDepartureCheckLanelet) +{ + const auto lanelet_map_ptr = route_handler->getLaneletMapPtr(); + + const geometry_msgs::msg::Pose goal_pose = + geometry_msgs::build() + .position(geometry_msgs::build() + .x(433.42254638671875) + .y(465.3381652832031) + .z(0.0)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(0.306785474523741) + .w(0.9517786888879384)); + + // 1) get target shoulder lane and check it's lane id + const auto target_shoulder_lane = route_handler->getPullOverTarget(goal_pose); + EXPECT_EQ(target_shoulder_lane.has_value(), true); + EXPECT_EQ(target_shoulder_lane.value().id(), 18391); + + // 2) get shoulder lane sequence + const auto target_shoulder_lanes = + route_handler->getShoulderLaneletSequence(target_shoulder_lane.value(), goal_pose); + EXPECT_EQ(target_shoulder_lanes.size(), 3); + + // 3) check if the right bound of the departure check lane extended to the right end matches the + // right bound of the rightmost lanelet + const auto to_points3d = [](const lanelet::ConstLineString3d & bound) { + lanelet::Points3d points; + for (const auto & pt : bound) { + points.push_back(lanelet::Point3d(pt)); + } + return points; + }; + + const auto departure_check_lane = + autoware::behavior_path_planner::goal_planner_utils::createDepartureCheckLanelet( + target_shoulder_lanes, *route_handler, true); + const auto departure_check_lane_right_bound_points = + to_points3d(departure_check_lane.rightBound()); + + const std::vector most_right_lanelet_ids = {18381, 18383, 18388}; + lanelet::Points3d right_bound_points; + for (const auto & id : most_right_lanelet_ids) { + const auto lanelet = lanelet_map_ptr->laneletLayer.get(id); + right_bound_points = autoware::behavior_path_planner::goal_planner_utils::combineLanePoints( + right_bound_points, to_points3d(lanelet.rightBound())); + } + + EXPECT_EQ(departure_check_lane_right_bound_points.size(), right_bound_points.size()); + for (size_t i = 0; i < departure_check_lane_right_bound_points.size(); ++i) { + EXPECT_EQ(departure_check_lane_right_bound_points.at(i).id(), right_bound_points.at(i).id()); + } +}