diff --git a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp index a93a94f296d45..77958c20a9e75 100644 --- a/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/src/load_info_from_lanelet2_map.cpp @@ -32,8 +32,24 @@ tier4_map_msgs::msg::MapProjectorInfo load_info_from_lanelet2_map(const std::str throw std::runtime_error("Error occurred while loading lanelet2 map"); } + // If the lat & lon values in all the points of lanelet2 map are all zeros, + // it will be interpreted as a local map. + // If any single point exists with non-zero lat or lon values, it will be interpreted as MGRS. + bool is_local = true; + for (const auto & point : map->pointLayer) { + const auto gps_point = projector.reverse(point); + if (gps_point.lat != 0.0 || gps_point.lon != 0.0) { + is_local = false; + break; + } + } + tier4_map_msgs::msg::MapProjectorInfo msg; - msg.type = "MGRS"; - msg.mgrs_grid = projector.getProjectedMGRSGrid(); + if (is_local) { + msg.type = "local"; + } else { + msg.type = "MGRS"; + msg.mgrs_grid = projector.getProjectedMGRSGrid(); + } return msg; } diff --git a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp index 52abacf67f344..206053fd2e8d2 100644 --- a/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp +++ b/map/map_projection_loader/test/test_load_info_from_lanelet2_map.cpp @@ -21,7 +21,7 @@ #include -void save_dummy_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) +void save_dummy_mgrs_lanelet2_map(const std::string & mgrs_coord, const std::string & output_path) { int zone = std::stoi(mgrs_coord.substr(0, 2)); bool is_north = false; @@ -47,13 +47,49 @@ void save_dummy_lanelet2_map(const std::string & mgrs_coord, const std::string & file.close(); } +void save_dummy_local_lanelet2_map(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + +void save_dummy_mgrs_lanelet2_map_with_one_zero_point(const std::string & output_path) +{ + std::ofstream file(output_path); + if (!file) { + std::cerr << "Unable to open file.\n"; + return; + } + + file << "\n"; + file << "\n"; + file << " \n"; + file << " \n"; + file << " \n"; + file << ""; + + file.close(); +} + TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) { // Save dummy lanelet2 map const std::string mgrs_grid = "54SUE"; const std::string mgrs_coord = mgrs_grid + "1000010000"; const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; - save_dummy_lanelet2_map(mgrs_coord, output_path); + save_dummy_mgrs_lanelet2_map(mgrs_coord, output_path); // Test the function const auto projector_info = load_info_from_lanelet2_map(output_path); @@ -63,6 +99,32 @@ TEST(TestLoadFromLanelet2Map, LoadMGRSGrid) EXPECT_EQ(projector_info.mgrs_grid, mgrs_grid); } +TEST(TestLoadFromLanelet2Map, LoadLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_local_lanelet2_map(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "local"); +} + +TEST(TestLoadFromLanelet2Map, LoadNoLocalGrid) +{ + // Save dummy lanelet2 map + const std::string output_path = "/tmp/test_load_info_from_lanelet2_map.osm"; + save_dummy_mgrs_lanelet2_map_with_one_zero_point(output_path); + + // Test the function + const auto projector_info = load_info_from_lanelet2_map(output_path); + + // Check the result + EXPECT_EQ(projector_info.type, "MGRS"); +} + int main(int argc, char ** argv) { ::testing::InitGoogleMock(&argc, argv);