feat(map_projection_loader): add backward compatibility for local map (…

…#4586) * feat(map_projection_loader): add backward compatibility for local map Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * feat: add dummy typo Signed-off-by: kminoda <koji.minoda@tier4.jp> * update test Signed-off-by: kminoda <koji.minoda@tier4.jp> * revert unnecessary fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * rename test Signed-off-by: kminoda <koji.minoda@tier4.jp> * fix Signed-off-by: kminoda <koji.minoda@tier4.jp> * style(pre-commit): autofix * fix comment Signed-off-by: kminoda <koji.minoda@tier4.jp> * remove comment Signed-off-by: kminoda <koji.minoda@tier4.jp> --------- Signed-off-by: kminoda <koji.minoda@tier4.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Aug 10, 2023 · 60d001e · 60d001e
1 parent c0a6c0f
commit 60d001e
Show file tree

Hide file tree

Showing 2 changed files with 82 additions and 4 deletions.
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
@@ -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
@@ -21,7 +21,7 @@
 
 #include <fstream>
 
-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 << "<?xml version=\"1.0\"?>\n";
+  file << "<osm version=\"0.6\" generator=\"lanelet2\">\n";
+  file << "  <node id=\"1\" lat=\"\" lon=\"\"/>\n";
+  file << "  <node id=\"2\" lat=\"\" lon=\"\"/>\n";
+  file << "  <node id=\"3\" lat=\"\" lon=\"\"/>\n";
+  file << "</osm>";
+
+  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 << "<?xml version=\"1.0\"?>\n";
+  file << "<osm version=\"0.6\" generator=\"lanelet2\">\n";
+  file << "  <node id=\"1\" lat=\"0.0\" lon=\"0.0\"/>\n";
+  file << "  <node id=\"2\" lat=\"0.00001\" lon=\"0.00001\"/>\n";
+  file << "  <node id=\"3\" lat=\"0.00002\" lon=\"0.00002\"/>\n";
+  file << "</osm>";
+
+  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);