fix: routing error

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

Author: mitsudome-r

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

@@ -48,6 +48,11 @@ bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
   GetDifferentialLanelet2Map::Request::SharedPtr req,
   GetDifferentialLanelet2Map::Response::SharedPtr res)
 {
+  if (!projector_info_) {
+    RCLCPP_ERROR(logger_, "Projector info is not set");
+    return false;
+  }
+
   // get the list of lanelet2 map paths from the requested cell_ids
   std::vector<std::string> lanelet2_paths;
   for (const auto & id : req->cell_ids) {
@@ -67,16 +72,23 @@ bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
     return false;
   }
 
-  // load the lanelet2 maps
-  lanelet::LaneletMapPtr map = std::make_shared<lanelet::LaneletMap>();
+  // load lanelet2 map
+  // We have to keep all loaded maps until publishing the map bin msg
+  // because the loaded lanelets will be expired when map is destructed
+  std::vector<lanelet::LaneletMapPtr> maps;
   for (const auto & path : lanelet2_paths) {
-    auto map_tmp = utils::load_map(path, projector_info_.value());
+    auto map_tmp = utils::load_map(path, *projector_info_);
     if (!map_tmp) {
-      RCLCPP_ERROR(
-        rclcpp::get_logger("map_loader"), "Failed to load lanelet2_map %s", path.c_str());
+      RCLCPP_ERROR(logger_, "Failed to load lanelet2_map, %s", path.c_str());
       return false;
     }
-    utils::merge_lanelet2_maps(*map, *map_tmp);
+    maps.push_back(map_tmp);
+  }
+
+  // merge all maps
+  lanelet::LaneletMapPtr map = std::make_shared<lanelet::LaneletMap>();
+  for (const auto & map_i : maps) {
+    utils::merge_lanelet2_maps(*map, *map_i);
   }
 
   // overwrite centerline

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -152,14 +152,22 @@ void Lanelet2MapLoaderNode::on_map_projector_info(
   }
 
   // load lanelet2 map
-  lanelet::LaneletMapPtr map = std::make_shared<lanelet::LaneletMap>();
+  // We have to keep all loaded maps until publishing the map bin msg
+  // because the loaded lanelets will be expired when map is destructed
+  std::vector<lanelet::LaneletMapPtr> maps;
   for (const auto & path : lanelet2_paths) {
     auto map_tmp = utils::load_map(path, *msg);
     if (!map_tmp) {
-      RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published.");
+      RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map, %s", path.c_str());
       return;
     }
-    utils::merge_lanelet2_maps(*map, *map_tmp);
+    maps.push_back(map_tmp);
+  }
+
+  // merge all maps
+  lanelet::LaneletMapPtr map = std::make_shared<lanelet::LaneletMap>();
+  for (const auto & map_i : maps) {
+    utils::merge_lanelet2_maps(*map, *map_i);
   }
 
   // we use first lanelet2 path to get format_version and map_version

map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp

@@ -141,19 +141,31 @@ void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap
   for (lanelet::Lanelet & lanelet : merge_source.laneletLayer) {
     merge_target.add(lanelet);
   }
-  for (const auto & area : merge_source.areaLayer) {
+
+  for (lanelet::Area & area : merge_source.areaLayer) {
     merge_target.add(area);
   }
-  for (const auto & regulatory_element : merge_source.regulatoryElementLayer) {
+
+  for (lanelet::RegulatoryElementPtr & regulatory_element : merge_source.regulatoryElementLayer) {
     merge_target.add(regulatory_element);
   }
-  for (const auto & line_string : merge_source.lineStringLayer) {
+
+  for (lanelet::LineString3d & line_string : merge_source.lineStringLayer) {
+    // we need a special handling for line_string to make sure that the points are not duplicated
+    // Otherwise, we cannot calculate the relationship of succeeding lanelets correctly
+    for (lanelet::Point3d & point : line_string) {
+      if (merge_target.pointLayer.find(point.id()) != merge_target.pointLayer.end()) {
+        point = merge_target.pointLayer.get(point.id());
+      }
+    }
     merge_target.add(line_string);
   }
-  for (const auto & polygon : merge_source.polygonLayer) {
+
+  for (lanelet::Polygon3d & polygon : merge_source.polygonLayer) {
     merge_target.add(polygon);
   }
-  for (const auto & point : merge_source.pointLayer) {
+
+  for (lanelet::Point3d & point : merge_source.pointLayer) {
     merge_target.add(point);
   }
 }