diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index 563350dbe53cc..d870bcf9974a1 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -19,6 +19,8 @@ #include #include +#include + #include namespace autoware::test_utils @@ -54,14 +56,32 @@ lanelet::LaneletMapPtr loadMap(const std::string & lanelet2_filename) lanelet::ErrorMessages errors{}; lanelet::projection::MGRSProjector projector{}; lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (errors.empty()) { - return map; + if (!errors.empty()) { + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; } - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + for (lanelet::Point3d point : map->pointLayer) { + if (point.hasAttribute("local_x")) { + point.x() = point.attribute("local_x").asDouble().value(); + } + if (point.hasAttribute("local_y")) { + point.y() = point.attribute("local_y").asDouble().value(); + } } - return nullptr; + + // realign lanelet borders using updated points + for (lanelet::Lanelet lanelet : map->laneletLayer) { + auto left = lanelet.leftBound(); + auto right = lanelet.rightBound(); + std::tie(left, right) = lanelet::geometry::align(left, right); + lanelet.setLeftBound(left); + lanelet.setRightBound(right); + } + + return map; } LaneletMapBin convertToMapBinMsg( diff --git a/planning/autoware_route_handler/test/test_route_handler.cpp b/planning/autoware_route_handler/test/test_route_handler.cpp index 35a2fe3ef45da..9a4081c51be3b 100644 --- a/planning/autoware_route_handler/test/test_route_handler.cpp +++ b/planning/autoware_route_handler/test/test_route_handler.cpp @@ -49,10 +49,11 @@ TEST_F(TestRouteHandler, getGoalLaneId) TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); + set_route_handler("overlap_map.osm"); ASSERT_FALSE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose start_pose, goal_pose; + geometry_msgs::msg::Pose start_pose; + geometry_msgs::msg::Pose goal_pose; start_pose.position = autoware::universe_utils::createPoint(3728.870361, 73739.281250, 0); start_pose.orientation = autoware::universe_utils::createQuaternion(0, 0, -0.513117, 0.858319); goal_pose.position = autoware::universe_utils::createPoint(3729.961182, 73727.328125, 0); @@ -79,11 +80,12 @@ TEST_F(TestRouteHandler, getLaneletSequenceWhenOverlappingRoute) TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) { - set_route_handler("/test_map/overlap_map.osm"); - set_test_route("/test_route/overlap_test_route.yaml"); + set_route_handler("overlap_map.osm"); + set_test_route("overlap_test_route.yaml"); ASSERT_TRUE(route_handler_->isHandlerReady()); - geometry_msgs::msg::Pose reference_pose, search_pose; + geometry_msgs::msg::Pose reference_pose; + geometry_msgs::msg::Pose search_pose; lanelet::ConstLanelet reference_lanelet; reference_pose.position = autoware::universe_utils::createPoint(3730.88, 73735.3, 0); @@ -102,71 +104,189 @@ TEST_F(TestRouteHandler, getClosestRouteLaneletFromLaneletWhenOverlappingRoute) ASSERT_EQ(closest_lanelet.id(), 345); found_lanelet = route_handler_->getClosestRouteLaneletFromLanelet( - search_pose, reference_lanelet, &closest_lanelet, 3.0, 1.046); + search_pose, reference_lanelet, &closest_lanelet, dist_threshold, yaw_threshold); ASSERT_TRUE(found_lanelet); ASSERT_EQ(closest_lanelet.id(), 277); } -// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) -// { -// lanelet::ConstLanelet closest_lane; - -// Pose search_pose; - -// search_pose.position = autoware::universe_utils::createPoint(-1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained7 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained7); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-0.5, 1.75, 0); -// const auto closest_lane_obtained = -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); - -// ASSERT_TRUE(closest_lane_obtained); -// ASSERT_EQ(closest_lane.id(), 4775); - -// search_pose.position = autoware::universe_utils::createPoint(-.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained3 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, CheckLaneIsInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4785); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_TRUE(is_lane_in_goal_route_section); +} -// ASSERT_TRUE(closest_lane_obtained3); -// ASSERT_EQ(closest_lane.id(), 4775); +TEST_F(TestRouteHandler, CheckLaneIsNotInGoalRouteSection) +{ + const auto lane = route_handler_->getLaneletsFromId(4780); + const auto is_lane_in_goal_route_section = route_handler_->isInGoalRouteSection(lane); + ASSERT_FALSE(is_lane_in_goal_route_section); +} -// search_pose.position = autoware::universe_utils::createPoint(0.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained1 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkGetLaneletSequence) +{ + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); -// ASSERT_TRUE(closest_lane_obtained1); -// ASSERT_EQ(closest_lane.id(), 4775); + lanelet::ConstLanelet closest_lanelet; + const auto found_closest_lanelet = route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + ASSERT_TRUE(found_closest_lanelet); + ASSERT_EQ(closest_lanelet.id(), 4765ul); + + const auto current_lanes = route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + + ASSERT_EQ(current_lanes.size(), 6ul); + ASSERT_EQ(current_lanes.at(0).id(), 4765ul); + ASSERT_EQ(current_lanes.at(1).id(), 4770ul); + ASSERT_EQ(current_lanes.at(2).id(), 4775ul); + ASSERT_EQ(current_lanes.at(3).id(), 4424ul); + ASSERT_EQ(current_lanes.at(4).id(), 4780ul); + ASSERT_EQ(current_lanes.at(5).id(), 4785ul); +} -// search_pose.position = autoware::universe_utils::createPoint(0.01, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained2 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneWhenLaneChangeToRight) +{ + const auto current_lanes = get_current_lanes(); + + // The input is within expectation. + // this lane is of preferred lane type + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // this alternative lane is a subset of preferred lane route section + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::RIGHT); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); + + // The input is within expectation. + // Although Direction::NONE, the function should still return result similar to + // Direction::RIGHT. + std::for_each(current_lanes.begin(), current_lanes.begin() + 3, [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 0ul); + }); + + // The input is within expectation. + // Although Direction::NONE is provided, the function should behave similarly to + // Direction::RIGHT. + std::for_each(current_lanes.begin() + 3, current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::NONE); + ASSERT_EQ(result.size(), 1ul); + EXPECT_DOUBLE_EQ(result.at(0), -3.5); + }); +} -// ASSERT_TRUE(closest_lane_obtained2); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, checkLateralIntervalToPreferredLaneUsingUnexpectedResults) +{ + const auto current_lanes = get_current_lanes(); -// search_pose.position = autoware::universe_utils::createPoint(0.5, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained4 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); + std::for_each(current_lanes.begin(), current_lanes.end(), [&](const auto & lane) { + const auto result = route_handler_->getLateralIntervalsToPreferredLane(lane, Direction::LEFT); + ASSERT_EQ(result.size(), 0ul); + }); +} -// ASSERT_TRUE(closest_lane_obtained4); -// ASSERT_EQ(closest_lane.id(), 4424); +TEST_F(TestRouteHandler, testGetCenterLinePath) +{ + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + { + // The input is within expectation. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 50.0); + ASSERT_EQ(center_line_path.points.size(), 51); // 26 + 26 - 1(overlapped) + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 2); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4780); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(1), 4785); + } + { + // The input is broken. + // s_start is negative, and s_end is over the boundary. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, -1.0, 200.0); + ASSERT_EQ(center_line_path.points.size(), 76); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4785); + } +} +TEST_F(TestRouteHandler, DISABLED_testGetCenterLinePathWhenLanesIsNotConnected) +{ + // broken current lanes. 4424 and 4785 are not connected directly. + const auto current_lanes = route_handler_->getLaneletsFromIds({4424, 4780, 4785}); + + // The input is broken. Test is disabled because it doesn't pass. + const auto center_line_path = route_handler_->getCenterLinePath(current_lanes, 0.0, 75.0); + ASSERT_EQ(center_line_path.points.size(), 26); // 26 + 26 + 26 - 2(overlapped) + ASSERT_EQ(center_line_path.points.front().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.front().lane_ids.at(0), 4424); + ASSERT_EQ(center_line_path.points.back().lane_ids.size(), 1); + ASSERT_EQ(center_line_path.points.back().lane_ids.at(0), 4424); +} -// search_pose.position = autoware::universe_utils::createPoint(1.0, 1.75, 0); -// search_pose.orientation = autoware::universe_utils::createQuaternion(0.0, 0.0, 0.0, 1.0); -// const auto closest_lane_obtained5 = -// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane); +TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute) +{ + auto get_closest_lanelet_within_route = + [&](double x, double y, double z) -> std::optional { + const auto pose = autoware::test_utils::createPose(x, y, z, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + const auto closest_lane_obtained = + route_handler_->getClosestLaneletWithinRoute(pose, &closest_lanelet); + if (!closest_lane_obtained) { + return std::nullopt; + } + return closest_lanelet.id(); + }; + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.5, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(-0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(-0.01, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.0, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.0, 1.75, 0).value(), 4775ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.01, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.01, 1.75, 0).value(), 4424ul); + + ASSERT_TRUE(get_closest_lanelet_within_route(0.5, 1.75, 0).has_value()); + ASSERT_EQ(get_closest_lanelet_within_route(0.5, 1.75, 0).value(), 4424ul); +} -// ASSERT_TRUE(closest_lane_obtained5); -// ASSERT_EQ(closest_lane.id(), 4424); -// } +TEST_F(TestRouteHandler, testGetLaneChangeTargetLanes) +{ + { + // The input is within expectation. + // There exist no lane changing lane since both 4770 and 4775 are preferred lane. + const auto current_lanes = route_handler_->getLaneletsFromIds({4770, 4775}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + ASSERT_FALSE(lane_change_lane.has_value()); + } + + { + // The input is within expectation. + // There exist lane changing lane since 4424 is subset of preferred lane 9598. + const auto current_lanes = route_handler_->getLaneletsFromIds({4775, 4424}); + const auto lane_change_lane = + route_handler_->getLaneChangeTarget(current_lanes, Direction::RIGHT); + EXPECT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } + + { + // The input is within expectation. + // There is a lane-changing lane. Within the maximum current lanes, there is an alternative lane + // to the preferred lane. Therefore, the lane-changing lane exists. + const auto current_lanes = get_current_lanes(); + const auto lane_change_lane = route_handler_->getLaneChangeTarget(current_lanes); + ASSERT_TRUE(lane_change_lane.has_value()); + ASSERT_EQ(lane_change_lane.value().id(), 9598ul); + } +} } // namespace autoware::route_handler::test diff --git a/planning/autoware_route_handler/test/test_route_handler.hpp b/planning/autoware_route_handler/test/test_route_handler.hpp index 86f7461fc7538..ec3368809df99 100644 --- a/planning/autoware_route_handler/test/test_route_handler.hpp +++ b/planning/autoware_route_handler/test/test_route_handler.hpp @@ -15,7 +15,6 @@ #ifndef TEST_ROUTE_HANDLER_HPP_ #define TEST_ROUTE_HANDLER_HPP_ -#include "ament_index_cpp/get_package_share_directory.hpp" #include "autoware_test_utils/autoware_test_utils.hpp" #include "autoware_test_utils/mock_data_parser.hpp" #include "gtest/gtest.h" @@ -37,16 +36,16 @@ namespace autoware::route_handler::test { +using autoware::test_utils::get_absolute_path_to_lanelet_map; +using autoware::test_utils::get_absolute_path_to_route; using autoware_map_msgs::msg::LaneletMapBin; - class TestRouteHandler : public ::testing::Test { public: TestRouteHandler() { - autoware_test_utils_dir = ament_index_cpp::get_package_share_directory("autoware_test_utils"); - set_route_handler("/test_map/2km_test.osm"); - set_test_route("/test_route/lane_change_test_route.yaml"); + set_route_handler("2km_test.osm"); + set_test_route(lane_change_right_test_route_filename); } TestRouteHandler(const TestRouteHandler &) = delete; @@ -55,26 +54,44 @@ class TestRouteHandler : public ::testing::Test TestRouteHandler & operator=(TestRouteHandler &&) = delete; ~TestRouteHandler() override = default; - void set_route_handler(const std::string & relative_path) + void set_route_handler(const std::string & lanelet_map_filename) { route_handler_.reset(); - const auto lanelet2_path = autoware_test_utils_dir + relative_path; + const auto lanelet2_path = + get_absolute_path_to_lanelet_map(autoware_test_utils_dir, lanelet_map_filename); const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(lanelet2_path, center_line_resolution); route_handler_ = std::make_shared(map_bin_msg); } - void set_test_route(const std::string & route_path) + void set_test_route(const std::string & route_filename) { - const auto route_handler_dir = - ament_index_cpp::get_package_share_directory("autoware_route_handler"); - const auto rh_test_route = route_handler_dir + route_path; + const auto rh_test_route = + get_absolute_path_to_route(autoware_route_handler_dir, route_filename); route_handler_->setRoute(autoware::test_utils::parse_lanelet_route_file(rh_test_route)); } + lanelet::ConstLanelets get_current_lanes() + { + const auto current_pose = autoware::test_utils::createPose(-50.0, 1.75, 0.0, 0.0, 0.0, 0.0); + lanelet::ConstLanelet closest_lanelet; + [[maybe_unused]] const auto found_closest_lanelet = + route_handler_->getClosestLaneletWithConstrainsWithinRoute( + current_pose, &closest_lanelet, dist_threshold, yaw_threshold); + return route_handler_->getLaneletSequence( + closest_lanelet, current_pose, backward_path_length, forward_path_length); + } + std::shared_ptr route_handler_; - std::string autoware_test_utils_dir; - static constexpr double center_line_resolution = 5.0; + std::string autoware_test_utils_dir{"autoware_test_utils"}; + std::string autoware_route_handler_dir{"autoware_route_handler"}; + std::string lane_change_right_test_route_filename{"lane_change_test_route.yaml"}; + + static constexpr double center_line_resolution{5.0}; + static constexpr double dist_threshold{3.0}; + static constexpr double yaw_threshold{1.045}; + static constexpr double backward_path_length{5.0}; + static constexpr double forward_path_length{300.0}; }; } // namespace autoware::route_handler::test