diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index c760ba1c94b45..3721dc67526b5 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp @@ -330,6 +330,19 @@ void updateNodeOptions( */ PathWithLaneId loadPathWithLaneIdInYaml(); +/** + * @brief create a straight lanelet from 2 segments defined by 4 points + * @param [in] left0 start of the left segment + * @param [in] left1 end of the left segment + * @param [in] right0 start of the right segment + * @param [in] right1 end of the right segment + * @return a ConstLanelet with the given left and right bounds and a unique lanelet id + * + */ +lanelet::ConstLanelet make_lanelet( + const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1, + const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1); + /** * @brief Generates a trajectory with specified parameters. * diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 1e2c7b50912f6..696f58d5e19e8 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -313,6 +313,19 @@ PathWithLaneId loadPathWithLaneIdInYaml() return parse<PathWithLaneId>(yaml_path); } +lanelet::ConstLanelet make_lanelet( + const lanelet::BasicPoint2d & left0, const lanelet::BasicPoint2d & left1, + const lanelet::BasicPoint2d & right0, const lanelet::BasicPoint2d & right1) +{ + lanelet::LineString3d left_bound; + left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left0.x(), left0.y(), 0.0)); + left_bound.push_back(lanelet::Point3d(lanelet::InvalId, left1.x(), left1.y(), 0.0)); + lanelet::LineString3d right_bound; + right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right0.x(), right0.y(), 0.0)); + right_bound.push_back(lanelet::Point3d(lanelet::InvalId, right1.x(), right1.y(), 0.0)); + return {lanelet::utils::getId(), left_bound, right_bound}; +} + std::optional<std::string> resolve_pkg_share_uri(const std::string & uri_path) { std::smatch match; diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt index bcc8dd9d9f299..22faf1a6375d8 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/CMakeLists.txt @@ -45,6 +45,7 @@ if(BUILD_TESTING) ament_add_ros_isolated_gmock(test_${PROJECT_NAME}_drivable_area_expansion test/test_drivable_area_expansion.cpp test/test_footprints.cpp + test/test_static_drivable_area.cpp ) target_link_libraries(test_${PROJECT_NAME}_drivable_area_expansion diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp index f393ec18f8a32..23f0de6f5348f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp @@ -25,12 +25,30 @@ namespace autoware::behavior_path_planner::utils { using autoware::behavior_path_planner::drivable_area_expansion::DrivableAreaExpansionParameters; +/** + * @brief finds the index of the first lane in the provided vector that overlaps with a preceding + * lane (excluding the immediate predecessor in the vector) + * @param [ink] lanes vector of DrivableLanes + * @return the index of the first overlapping lane (if any) + */ std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes); +/** + * @brief modify a path to only keep points inside the given lanes (before any lane overlap) + * @details the path point lanelet_ids are used to determine if they are inside a lanelet + * @param [inout] path path to be cut + * @param [in] lanes lanes used to cut the path + * @return the shortened lanes without overlaps + */ std::vector<DrivableLanes> cutOverlappedLanes( PathWithLaneId & path, const std::vector<DrivableLanes> & lanes); -std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & current_lanes); +/** + * @brief generate DrivableLanes objects from a sequence of lanelets + * @param [in] lanelets sequence of lanelets + * @return a vector of DrivableLanes constructed from the given lanelets + */ +std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanelets); std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & shoulder_lanes); @@ -38,12 +56,36 @@ std::vector<DrivableLanes> generateDrivableLanesWithShoulderLanes( std::vector<DrivableLanes> getNonOverlappingExpandedLanes( PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const DrivableAreaExpansionParameters & parameters); + +/** + * @brief generate the drivable area of the given path + * @param [inout] path path whose center line is used to calculate the drivable area and whose + * left/right bound are generated + * @param [in] lanes lanes driven by the ego vehicle + * @param [in] enable_expanding_hatched_road_markings if true, expand the drivable area into hatched + * road markings + * @param [in] enable_expanding_intersection_areas if true, expand the drivable area into + * intersection areas + * @param [in] enable_expanding_freespace_areas if true, expand the drivable area into freespace + * areas + * @param [in] planner_data planner data with the route handler (and map), the parameters, the ego + * pose, etc + * @param [in] is_driving_forward true if the ego vehicle drives in the forward direction + */ void generateDrivableArea( PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const bool enable_expanding_hatched_road_markings, const bool enable_expanding_intersection_areas, const bool enable_expanding_freespace_areas, const std::shared_ptr<const PlannerData> planner_data, const bool is_driving_forward = true); +/** + * @brief generate the drivable area of the given path by applying the given offsets to its points + * @param [inout] path path whose center line is used to calculate the drivable area and whose + * left/right bound are generated + * @param [in] vehicle_length [m] length of the ego vehicle + * @param [in] offset [m] lateral offset between the path points and the drivable area + * @param [in] is_driving_forward true if the ego vehicle drives in the forward direction + */ void generateDrivableArea( PathWithLaneId & path, const double vehicle_length, const double offset, const bool is_driving_forward = true); @@ -72,6 +114,14 @@ std::vector<lanelet::ConstPoint3d> getBoundWithHatchedRoadMarkings( const std::vector<lanelet::ConstPoint3d> & original_bound, const std::shared_ptr<RouteHandler> & route_handler); +/** + * @brief Expand the given bound to include intersection areas from the map + * @param [in] original_bound original bound to expand + * @param [in] drivable_lanes lanelets to consider + * @param [in] route_handler route handler with the map information + * @param [in] is_left whether the bound to calculate is on the left or not + * @return the bound including intersection areas + */ std::vector<lanelet::ConstPoint3d> getBoundWithIntersectionAreas( const std::vector<lanelet::ConstPoint3d> & original_bound, const std::shared_ptr<RouteHandler> & route_handler, @@ -90,6 +140,12 @@ std::vector<geometry_msgs::msg::Point> postProcess( const std::vector<DrivableLanes> & drivable_lanes, const bool is_left, const bool is_driving_forward = true); +/** + * @brief combine two drivable area info objects + * @param [in] drivable_area_Info1 first drivable area info + * @param [in] drivable_area_Info2 second drivable area info + * @return the combined drivable area info + */ DrivableAreaInfo combineDrivableAreaInfo( const DrivableAreaInfo & drivable_area_info1, const DrivableAreaInfo & drivable_area_info2); diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp index 9c35032e51063..012c42a379c00 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/drivable_area_expansion/static_drivable_area.cpp @@ -626,8 +626,6 @@ std::vector<Point> updateBoundary( namespace autoware::behavior_path_planner::utils { -using autoware::universe_utils::Point2d; - std::optional<size_t> getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes) { auto overlaps = [](const DrivableLanes & lanes, const DrivableLanes & target_lanes) { @@ -745,12 +743,12 @@ std::vector<DrivableLanes> cutOverlappedLanes( return shorten_lanes; } -std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanes) +std::vector<DrivableLanes> generateDrivableLanes(const lanelet::ConstLanelets & lanelets) { - std::vector<DrivableLanes> drivable_lanes(lanes.size()); - for (size_t i = 0; i < lanes.size(); ++i) { - drivable_lanes.at(i).left_lane = lanes.at(i); - drivable_lanes.at(i).right_lane = lanes.at(i); + std::vector<DrivableLanes> drivable_lanes(lanelets.size()); + for (size_t i = 0; i < lanelets.size(); ++i) { + drivable_lanes.at(i).left_lane = lanelets.at(i); + drivable_lanes.at(i).right_lane = lanelets.at(i); } return drivable_lanes; } @@ -856,6 +854,9 @@ void generateDrivableArea( } } } + if (resampled_path.points.empty()) { + return; + } // add last point of path if enough far from the one of resampled path constexpr double th_last_point_distance = 0.3; if ( diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp new file mode 100644 index 0000000000000..ba3141a89b26a --- /dev/null +++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/test/test_static_drivable_area.cpp @@ -0,0 +1,528 @@ +// 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 "autoware/behavior_path_planner_common/data_manager.hpp" +#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" + +#include <autoware/route_handler/route_handler.hpp> +#include <autoware_test_utils/autoware_test_utils.hpp> + +#include <nav_msgs/msg/detail/odometry__struct.hpp> +#include <tier4_planning_msgs/msg/path_with_lane_id.hpp> + +#include <boost/geometry/algorithms/detail/disjoint/interface.hpp> +#include <boost/geometry/algorithms/detail/intersects/interface.hpp> + +#include <gtest/gtest.h> +#include <lanelet2_core/Forward.h> +#include <lanelet2_core/LaneletMap.h> +#include <lanelet2_core/primitives/Lanelet.h> +#include <lanelet2_core/primitives/LineString.h> +#include <lanelet2_core/primitives/Point.h> + +#include <memory> + +const auto intersection_map = + autoware::test_utils::make_map_bin_msg(autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "intersection/lanelet2_map.osm")); + +using autoware::behavior_path_planner::DrivableLanes; +using autoware::test_utils::make_lanelet; + +DrivableLanes make_drivable_lanes(const lanelet::ConstLanelet & ll) +{ + using autoware::behavior_path_planner::utils::generateDrivableLanes; + return generateDrivableLanes({ll}).front(); +} + +bool equal(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + return p1.x == p2.x && p1.y == p2.y && p1.z == p2.z; +} + +bool equal(const DrivableLanes & l1, const DrivableLanes & l2) +{ + if (l1.middle_lanes.size() != l2.middle_lanes.size()) { + return false; + } + auto are_equal = true; + are_equal &= boost::geometry::equals( + l1.left_lane.polygon2d().basicPolygon(), l2.left_lane.polygon2d().basicPolygon()); + are_equal &= boost::geometry::equals( + l1.right_lane.polygon2d().basicPolygon(), l2.right_lane.polygon2d().basicPolygon()); + for (auto i = 0UL; i < l1.middle_lanes.size(); ++i) { + are_equal &= boost::geometry::equals( + l1.middle_lanes[i].polygon2d().basicPolygon(), l2.middle_lanes[i].polygon2d().basicPolygon()); + } + return are_equal; +} + +TEST(StaticDrivableArea, getOverlappedLaneletId) +{ + using autoware::behavior_path_planner::utils::getOverlappedLaneletId; + + std::vector<DrivableLanes> lanes; + { // empty lanes + const auto result = getOverlappedLaneletId(lanes); + EXPECT_FALSE(result.has_value()); + } + { // lanes at 0 + const DrivableLanes l = + make_drivable_lanes(make_lanelet({0.0, 1.0}, {5.0, 1.0}, {5.0, -1.0}, {5.0, -1.0})); + lanes.push_back(l); + const auto result = getOverlappedLaneletId(lanes); + EXPECT_FALSE(result.has_value()); + } + { // lanes at 1, overlap with 0 but ignored since it is the following lane + const DrivableLanes l = + make_drivable_lanes(make_lanelet({4.0, 1.0}, {8.0, 1.0}, {4.0, -1.0}, {8.0, -1.0})); + lanes.push_back(l); + const auto result = getOverlappedLaneletId(lanes); + EXPECT_FALSE(result.has_value()); + } + { // lanes at 2, overlap with 1 but ignored since it is the following lane + const DrivableLanes l = + make_drivable_lanes(make_lanelet({6.0, 1.0}, {10.0, 1.0}, {6.0, -1.0}, {10.0, -1.0})); + lanes.push_back(l); + const auto result = getOverlappedLaneletId(lanes); + EXPECT_FALSE(result.has_value()); + } + { // lanes at 3, overlap with 1 so 3 is returned + const DrivableLanes l = + make_drivable_lanes(make_lanelet({5.0, 0.0}, {5.0, 5.0}, {6.0, 0.0}, {6.0, 5.0})); + lanes.push_back(l); + const auto result = getOverlappedLaneletId(lanes); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(*result, 3UL); + } + { // lanes at 4, overlap with 2 but since 3 overlaps first it is still returned + const DrivableLanes l = + make_drivable_lanes(make_lanelet({7.0, 0.0}, {7.0, 5.0}, {8.0, 0.0}, {8.0, 5.0})); + lanes.push_back(l); + const auto result = getOverlappedLaneletId(lanes); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(*result, 3UL); + } + { // change 1 to no longer overlap with 3 and now 4 is the first overlap + const DrivableLanes l = make_drivable_lanes( + make_lanelet({100.0, 110.0}, {110.0, 100.0}, {100.0, 90.0}, {100.0, 90.0})); + lanes[1] = l; + const auto result = getOverlappedLaneletId(lanes); + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(*result, 4UL); + } +} + +TEST(StaticDrivableArea, cutOverlappedLanes) +{ + using autoware::behavior_path_planner::utils::cutOverlappedLanes; + tier4_planning_msgs::msg::PathWithLaneId path; + std::vector<DrivableLanes> lanes; + { // empty inputs + const auto result = cutOverlappedLanes(path, lanes); + EXPECT_TRUE(result.empty()); + EXPECT_TRUE(path.points.empty()); + } + constexpr auto path_size = 10UL; + const auto reset_path = [&]() { + path.points.clear(); + for (auto i = 0UL; i < path_size; ++i) { + path.points.emplace_back(); + path.points.back().point.pose.position.x = static_cast<double>(i) * 1.0; + path.points.back().point.pose.position.y = 0.0; + } + }; + { // add some path points + reset_path(); + const auto result = cutOverlappedLanes(path, lanes); + EXPECT_TRUE(result.empty()); + ASSERT_EQ(path.points.size(), path_size); + for (auto i = 0UL; i < path_size; ++i) { + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.x, i * 1.0); + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.y, 0.0); + } + } + { // add some drivable lanes without any overlap (no overlap -> path is not modified) + reset_path(); + lanes.push_back( + make_drivable_lanes(make_lanelet({0.0, 1.0}, {2.0, 1.0}, {0.0, -1.0}, {2.0, -1.0}))); + const auto result = cutOverlappedLanes(path, lanes); + ASSERT_EQ(result.size(), lanes.size()); + EXPECT_TRUE(equal(result.front(), lanes.front())); + ASSERT_EQ(path.points.size(), path_size); + for (auto i = 0UL; i < path_size; ++i) { + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.x, i * 1.0); + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.y, 0.0); + } + } + { // add more drivable lanes without an overlap (no overlap -> path is not modified) + reset_path(); + lanes.push_back( + make_drivable_lanes(make_lanelet({2.0, 1.0}, {4.0, 1.0}, {2.0, -1.0}, {4.0, -1.0}))); + lanes.push_back( + make_drivable_lanes(make_lanelet({4.0, 1.0}, {6.0, 1.0}, {4.0, -1.0}, {6.0, -1.0}))); + const auto result = cutOverlappedLanes(path, lanes); + ASSERT_EQ(result.size(), lanes.size()); + for (auto i = 0UL; i < result.size(); ++i) { + EXPECT_TRUE(equal(result[i], lanes[i])); + } + ASSERT_EQ(path.points.size(), path_size); + for (auto i = 0UL; i < path_size; ++i) { + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.x, i * 1.0); + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.y, 0.0); + } + } + { // add an overlapping lane + reset_path(); + lanes.push_back( + make_drivable_lanes(make_lanelet({2.5, -1.0}, {2.5, 1.0}, {3.5, -1.0}, {3.5, 1.0}))); + const auto result = cutOverlappedLanes(path, lanes); + // the last lane is cut + ASSERT_EQ(result.size() + 1, lanes.size()); + for (auto i = 0UL; i < result.size(); ++i) { + EXPECT_TRUE(equal(result[i], lanes[i])); + } + // since the path points do not have ids, all points are cut + EXPECT_TRUE(path.points.empty()); + } + { // add the overlapping lane id to the path points + reset_path(); + for (auto & p : path.points) { + p.lane_ids.push_back(lanes.back().left_lane.id()); + } + cutOverlappedLanes(path, lanes); + // since the overlapped lane was removed, the path points were still cut + EXPECT_TRUE(path.points.empty()); + } + { // add the first lane id to some path points, only these points will be kept + reset_path(); + constexpr auto filtered_start = 3UL; + constexpr auto filtered_size = 5UL; + for (auto i = filtered_start; i < filtered_start + filtered_size; ++i) { + path.points[i].lane_ids.push_back(lanes.front().left_lane.id()); + } + cutOverlappedLanes(path, lanes); + ASSERT_EQ(path.points.size(), filtered_size); + for (auto i = 0UL; i < filtered_size; ++i) { + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.x, (i + filtered_start) * 1.0); + EXPECT_DOUBLE_EQ(path.points[i].point.pose.position.y, 0.0); + } + } +} + +TEST(StaticDrivableArea, generateDrivableLanes) +{ + using autoware::behavior_path_planner::utils::generateDrivableLanes; + lanelet::ConstLanelets lanelets; + lanelet::Lanelet ll; + { // empty case + const auto lanes = generateDrivableLanes(lanelets); + EXPECT_TRUE(lanes.empty()); + } + { // single lanelet: it is found in the drivable lane's left/right lanes + ll.setId(0); + lanelets.push_back(ll); + const auto lanes = generateDrivableLanes(lanelets); + ASSERT_EQ(lanes.size(), lanelets.size()); + EXPECT_TRUE(lanes[0].middle_lanes.empty()); + EXPECT_EQ(lanes[0].left_lane.id(), lanelets[0].id()); + EXPECT_EQ(lanes[0].right_lane.id(), lanelets[0].id()); + } + { // many lanelets: they are all in the calculated drivable lane + for (auto i = 1; i < 20; ++i) { + ll.setId(0); + lanelets.push_back(ll); + } + const auto lanes = generateDrivableLanes(lanelets); + ASSERT_EQ(lanes.size(), lanelets.size()); + for (auto i = 0UL; i < lanes.size(); ++i) { + EXPECT_TRUE(lanes[i].middle_lanes.empty()); + EXPECT_EQ(lanes[i].left_lane.id(), lanelets[i].id()); + EXPECT_EQ(lanes[i].right_lane.id(), lanelets[i].id()); + } + } +} + +TEST(StaticDrivableArea, generateDrivableArea_subfunction) +{ + using autoware::behavior_path_planner::utils::generateDrivableArea; + tier4_planning_msgs::msg::PathWithLaneId path; + tier4_planning_msgs::msg::PathPointWithLaneId p; + generateDrivableArea(path, 0.0, 0.0, true); + EXPECT_TRUE(path.left_bound.empty()); + EXPECT_TRUE(path.right_bound.empty()); + // add only 1 point : drivable area with 1 point + p.point.pose.position.set__x(0.0).set__y(0.0); + path.points.push_back(p); + auto lon_offset = 0.0; + auto lat_offset = 0.0; + generateDrivableArea(path, lon_offset, lat_offset, true); + // 3 points in the resulting drivable area: 1 for the path point and 2 for front/rear offset + ASSERT_EQ(path.left_bound.size(), 3UL); + ASSERT_EQ(path.right_bound.size(), 3UL); + // no offset so we expect exactly the same points + for (auto i = 0UL; i < 3UL; ++i) { + EXPECT_TRUE(equal(path.points.front().point.pose.position, path.left_bound[i])); + EXPECT_TRUE(equal(path.points.front().point.pose.position, path.right_bound[i])); + } + // add some offset + lon_offset = 1.0; + lat_offset = 0.5; + generateDrivableArea(path, lon_offset, lat_offset, true); + ASSERT_EQ(path.left_bound.size(), 3UL); + ASSERT_EQ(path.right_bound.size(), 3UL); + EXPECT_DOUBLE_EQ(path.left_bound[0].x, -lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.left_bound[2].x, lon_offset); + EXPECT_DOUBLE_EQ(path.right_bound[0].x, -lon_offset); + EXPECT_DOUBLE_EQ(path.right_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.right_bound[2].x, lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound[0].y, lat_offset); + EXPECT_DOUBLE_EQ(path.left_bound[1].y, lat_offset); + EXPECT_DOUBLE_EQ(path.left_bound[2].y, lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[0].y, -lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[1].y, -lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[2].y, -lat_offset); + // set driving_forward to false: longitudinal offset is inversely applied + generateDrivableArea(path, lon_offset, lat_offset, false); + ASSERT_EQ(path.left_bound.size(), 3UL); + ASSERT_EQ(path.right_bound.size(), 3UL); + EXPECT_DOUBLE_EQ(path.left_bound[0].x, lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.left_bound[2].x, -lon_offset); + EXPECT_DOUBLE_EQ(path.right_bound[0].x, lon_offset); + EXPECT_DOUBLE_EQ(path.right_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.right_bound[2].x, -lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound[0].y, lat_offset); + EXPECT_DOUBLE_EQ(path.left_bound[1].y, lat_offset); + EXPECT_DOUBLE_EQ(path.left_bound[2].y, lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[0].y, -lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[1].y, -lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[2].y, -lat_offset); + // add more points + for (auto x = 1; x < 10; ++x) { + // space points by more than 2m to avoid resampling + p.point.pose.position.set__x(x * 3.0).set__y(0.0); + path.points.push_back(p); + } + generateDrivableArea(path, lon_offset, lat_offset, true); + ASSERT_EQ(path.left_bound.size(), path.points.size() + 2UL); + ASSERT_EQ(path.right_bound.size(), path.points.size() + 2UL); + EXPECT_DOUBLE_EQ(path.left_bound.front().x, -lon_offset); + EXPECT_DOUBLE_EQ(path.right_bound.front().x, -lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound.back().x, path.points.back().point.pose.position.x + lon_offset); + EXPECT_DOUBLE_EQ( + path.right_bound.back().x, path.points.back().point.pose.position.x + lon_offset); + EXPECT_DOUBLE_EQ(path.left_bound.front().y, lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound.front().y, -lat_offset); + EXPECT_DOUBLE_EQ(path.left_bound.back().y, lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound.back().y, -lat_offset); + for (auto i = 1UL; i + 1 < path.points.size(); ++i) { + const auto & path_p = path.points[i - 1].point.pose.position; + EXPECT_DOUBLE_EQ(path.left_bound[i].x, path_p.x); + EXPECT_DOUBLE_EQ(path.right_bound[i].x, path_p.x); + EXPECT_DOUBLE_EQ(path.left_bound[i].y, path_p.y + lat_offset); + EXPECT_DOUBLE_EQ(path.right_bound[i].y, path_p.y - lat_offset); + } + // case with self intersections + path.points.clear(); + p.point.pose.position.set__x(0.0).set__y(0.0); + path.points.push_back(p); + p.point.pose.position.set__x(3.0).set__y(0.0); + path.points.push_back(p); + p.point.pose.position.set__x(0.0).set__y(3.0); + path.points.push_back(p); + lon_offset = 0.0; + lat_offset = 3.0; + generateDrivableArea(path, lon_offset, lat_offset, false); + // TODO(Anyone): self intersection case looks buggy + ASSERT_EQ(path.left_bound.size(), path.points.size() + 2UL); + ASSERT_EQ(path.right_bound.size(), path.points.size() + 2UL); + EXPECT_TRUE(equal(path.left_bound[0], path.left_bound[1])); + EXPECT_TRUE(equal(path.right_bound[0], path.right_bound[1])); + EXPECT_TRUE(equal(path.left_bound[3], path.left_bound[4])); + EXPECT_TRUE(equal(path.right_bound[3], path.right_bound[4])); + EXPECT_DOUBLE_EQ(path.left_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.left_bound[1].y, 3.0); + EXPECT_DOUBLE_EQ(path.left_bound[2].x, 3.0); + EXPECT_DOUBLE_EQ(path.left_bound[2].y, 3.0); + EXPECT_DOUBLE_EQ(path.left_bound[3].x, 0.0); + EXPECT_DOUBLE_EQ(path.left_bound[3].y, 6.0); + EXPECT_DOUBLE_EQ(path.right_bound[1].x, 0.0); + EXPECT_DOUBLE_EQ(path.right_bound[1].y, -3.0); + EXPECT_DOUBLE_EQ(path.right_bound[2].x, 3.0); + EXPECT_DOUBLE_EQ(path.right_bound[2].y, -3.0); + EXPECT_DOUBLE_EQ(path.right_bound[3].x, 0.0); + EXPECT_DOUBLE_EQ(path.right_bound[3].y, 0.0); +} + +TEST(StaticDrivableArea, getBoundWithIntersectionAreas) +{ + using autoware::behavior_path_planner::utils::getBoundWithIntersectionAreas; + std::vector<lanelet::ConstPoint3d> original_bound; + std::shared_ptr<autoware::route_handler::RouteHandler> route_handler = + std::make_shared<autoware::route_handler::RouteHandler>(); + std::vector<DrivableLanes> drivable_lanes; + bool is_left = false; + auto result = + getBoundWithIntersectionAreas(original_bound, route_handler, drivable_lanes, is_left); + EXPECT_TRUE(result.empty()); + + route_handler->setMap(intersection_map); + DrivableLanes lanes; + const auto lanelet_with_intersection_area = route_handler->getLaneletsFromId(3101); + lanes.middle_lanes = {}; + lanes.right_lane = lanelet_with_intersection_area; + lanes.left_lane = lanelet_with_intersection_area; + for (const auto & p : lanelet_with_intersection_area.rightBound()) { + original_bound.push_back(p); + } + drivable_lanes.push_back(lanes); + result = getBoundWithIntersectionAreas(original_bound, route_handler, drivable_lanes, is_left); + // the expanded bound includes the intersection area so its size is larger + EXPECT_GT(result.size(), original_bound.size()); +} + +TEST(StaticDrivableArea, combineDrivableAreaInfo) +{ + using autoware::behavior_path_planner::utils::combineDrivableAreaInfo; + autoware::behavior_path_planner::DrivableAreaInfo drivable_area_info1; + autoware::behavior_path_planner::DrivableAreaInfo drivable_area_info2; + { + const auto combined = combineDrivableAreaInfo(drivable_area_info1, drivable_area_info2); + EXPECT_TRUE(combined.drivable_lanes.empty()); + } + { // combination of obstacles + drivable_area_info1.obstacles.emplace_back().pose.position.x = 1.0; + drivable_area_info2.obstacles.emplace_back().pose.position.x = 2.0; + const auto combined = combineDrivableAreaInfo(drivable_area_info1, drivable_area_info2); + ASSERT_EQ(combined.obstacles.size(), 2UL); + EXPECT_DOUBLE_EQ(combined.obstacles[0].pose.position.x, 1.0); + EXPECT_DOUBLE_EQ(combined.obstacles[1].pose.position.x, 2.0); + } + { // combination of the drivable lanes + DrivableLanes lanes; + lanes.middle_lanes.push_back(lanelet::Lanelet(0)); + lanes.middle_lanes.push_back(lanelet::Lanelet(1)); + drivable_area_info1.drivable_lanes.push_back(lanes); + lanes.middle_lanes.clear(); + lanes.middle_lanes.push_back(lanelet::Lanelet(2)); + lanes.middle_lanes.push_back(lanelet::Lanelet(3)); + drivable_area_info2.drivable_lanes.push_back(lanes); + const auto combined = combineDrivableAreaInfo(drivable_area_info1, drivable_area_info2); + ASSERT_EQ(combined.drivable_lanes.size(), 1UL); + ASSERT_EQ(combined.drivable_lanes[0].middle_lanes.size(), 4UL); + for (auto id = 0UL; id < combined.drivable_lanes[0].middle_lanes.size(); ++id) { + EXPECT_EQ(combined.drivable_lanes[0].middle_lanes[id].id(), id); + } + } + { // combination of the parameters + drivable_area_info1.drivable_margin = 5.0; + drivable_area_info2.drivable_margin = 2.0; + drivable_area_info1.enable_expanding_freespace_areas = false; + drivable_area_info2.enable_expanding_freespace_areas = false; + drivable_area_info1.enable_expanding_intersection_areas = true; + drivable_area_info2.enable_expanding_intersection_areas = true; + drivable_area_info1.enable_expanding_hatched_road_markings = false; + drivable_area_info2.enable_expanding_hatched_road_markings = true; + const auto combined = combineDrivableAreaInfo(drivable_area_info1, drivable_area_info2); + EXPECT_DOUBLE_EQ(combined.drivable_margin, 5.0); // expect the maximum of the margins + // expect OR of the booleans + EXPECT_FALSE(combined.enable_expanding_freespace_areas); + EXPECT_TRUE(combined.enable_expanding_intersection_areas); + EXPECT_TRUE(combined.enable_expanding_hatched_road_markings); + } +} + +TEST(DISABLED_StaticDrivableArea, generateDrivableArea) +{ + using autoware::behavior_path_planner::PlannerData; + using autoware::behavior_path_planner::utils::generateDrivableArea; + tier4_planning_msgs::msg::PathWithLaneId path; + std::vector<DrivableLanes> lanes; + bool enable_expanding_hatched_road_markings = true; + bool enable_expanding_intersection_areas = true; + bool enable_expanding_freespace_areas = true; + bool is_driving_forward = true; + PlannerData planner_data; + planner_data.drivable_area_expansion_parameters.enabled = false; // disable dynamic expansion + planner_data.parameters.ego_nearest_dist_threshold = 1.0; + planner_data.parameters.ego_nearest_yaw_threshold = M_PI; + auto planner_data_ptr = std::make_shared<PlannerData>(planner_data); + // empty: no crash + EXPECT_NO_FATAL_FAILURE(generateDrivableArea( + path, lanes, enable_expanding_hatched_road_markings, enable_expanding_intersection_areas, + enable_expanding_freespace_areas, planner_data_ptr, is_driving_forward)); + planner_data.route_handler = std::make_shared<autoware::route_handler::RouteHandler>(); + planner_data.route_handler->setMap(intersection_map); + // create a path from a lanelet centerline + constexpr auto lanelet_id = 3008377; + const auto ll = planner_data.route_handler->getLaneletsFromId(lanelet_id); + const auto shoulder_ll = planner_data.route_handler->getLaneletsFromId(3008385); + lanes = autoware::behavior_path_planner::utils::generateDrivableLanes({ll, shoulder_ll}); + lanelet::BasicLineString2d path_ls; + for (const auto & p : ll.centerline().basicLineString()) { + tier4_planning_msgs::msg::PathPointWithLaneId pp; + pp.point.pose.position.x = p.x(); + pp.point.pose.position.y = p.y(); + pp.point.pose.position.z = p.z(); + pp.lane_ids = {lanelet_id}; + path.points.push_back(pp); + path_ls.emplace_back(p.x(), p.y()); + } + auto odometry = nav_msgs::msg::Odometry(); + odometry.pose.pose = path.points.front().point.pose; + planner_data.self_odometry = std::make_shared<nav_msgs::msg::Odometry>(odometry); + planner_data_ptr = std::make_shared<PlannerData>(planner_data); + generateDrivableArea( + path, lanes, enable_expanding_hatched_road_markings, enable_expanding_intersection_areas, + enable_expanding_freespace_areas, planner_data_ptr, is_driving_forward); + + ASSERT_EQ(path.left_bound.size(), ll.leftBound().size()); + ASSERT_EQ(path.right_bound.size(), ll.rightBound().size()); + { + lanelet::BasicLineString2d left_ls; + lanelet::BasicLineString2d right_ls; + for (const auto & p : path.left_bound) { + left_ls.emplace_back(p.x, p.y); + } + for (const auto & p : path.right_bound) { + right_ls.emplace_back(p.x, p.y); + } + EXPECT_FALSE(boost::geometry::intersects(path_ls, left_ls)); + EXPECT_FALSE(boost::geometry::intersects(path_ls, right_ls)); // TODO(someone): this is failing + } + // reverse case + is_driving_forward = false; + odometry.pose.pose = std::prev(path.points.end(), 2)->point.pose; + planner_data.self_odometry = std::make_shared<nav_msgs::msg::Odometry>(odometry); + planner_data_ptr = std::make_shared<PlannerData>(planner_data); + generateDrivableArea( + path, lanes, enable_expanding_hatched_road_markings, enable_expanding_intersection_areas, + enable_expanding_freespace_areas, planner_data_ptr, is_driving_forward); + ASSERT_EQ(path.left_bound.size(), ll.leftBound().size()); + ASSERT_EQ(path.right_bound.size(), ll.rightBound().size()); + { + lanelet::BasicLineString2d left_ls; + lanelet::BasicLineString2d right_ls; + for (const auto & p : path.left_bound) { + left_ls.emplace_back(p.x, p.y); + } + for (const auto & p : path.right_bound) { + right_ls.emplace_back(p.x, p.y); + } + EXPECT_FALSE(boost::geometry::intersects(path_ls, left_ls)); + EXPECT_FALSE(boost::geometry::intersects(path_ls, right_ls)); + } +}