From e56ca6749f504fd3ad81e2db13fd6c551443f6fa Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 11 Dec 2024 07:26:12 +0900 Subject: [PATCH 1/2] add the empty route test and empty map test Signed-off-by: Zhe Shen --- .../test/src/test_route_checker.cpp | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/autoware_lanelet2_extension/test/src/test_route_checker.cpp b/autoware_lanelet2_extension/test/src/test_route_checker.cpp index 11ab65e..7347336 100644 --- a/autoware_lanelet2_extension/test/src/test_route_checker.cpp +++ b/autoware_lanelet2_extension/test/src/test_route_checker.cpp @@ -91,6 +91,24 @@ TEST_F(TestSuite, isRouteValid) // NOLINT for gtest "one"; } +TEST_F(TestSuite, isRouteInvalidWithEmptyRoute) // NOLINT for gtest +{ + // create empty route + autoware_planning_msgs::msg::LaneletRoute empty_route; + + ASSERT_FALSE(lanelet::utils::route::isRouteValid(empty_route, sample_map_ptr)) + << "The route should be invalid because it is empty."; +} + +TEST_F(TestSuite, isRouteInvalidWithEmptyMap) // NOLINT for gtest +{ + // create empty map + auto empty_map_ptr = std::make_shared(); + + ASSERT_FALSE(lanelet::utils::route::isRouteValid(sample_route1, empty_map_ptr)) + << "The route should be invalid because the map is empty."; +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); From 5ff6392ffa6101fd8c7aa6f1476a8679724bcd9a Mon Sep 17 00:00:00 2001 From: Zhe Shen Date: Wed, 11 Dec 2024 22:39:48 +0100 Subject: [PATCH 2/2] add the empty checker to isRouteValid() Signed-off-by: Zhe Shen --- autoware_lanelet2_extension/lib/route_checker.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/autoware_lanelet2_extension/lib/route_checker.cpp b/autoware_lanelet2_extension/lib/route_checker.cpp index 7a6269a..1f4ae3b 100644 --- a/autoware_lanelet2_extension/lib/route_checker.cpp +++ b/autoware_lanelet2_extension/lib/route_checker.cpp @@ -26,6 +26,10 @@ namespace lanelet::utils bool route::isRouteValid( const LaneletRoute & route_msg, const lanelet::LaneletMapPtr lanelet_map_ptr_) { + if (route_msg.segments.empty()) { + std::cerr << "Route is empty, returning false." << std::endl; + return false; + } for (const auto & route_section : route_msg.segments) { for (const auto & primitive : route_section.primitives) { const auto id = primitive.id;