Add differential lanelet loading

Signed-off-by: Barış Zeren <[email protected]>

map/map_loader/CMakeLists.txt

@@ -29,6 +29,8 @@ rclcpp_components_register_node(pointcloud_map_loader_node
 
 ament_auto_add_library(lanelet2_map_loader_node SHARED
   src/lanelet2_map_loader/lanelet2_map_loader_node.cpp
+  src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp
+  src/lanelet2_map_loader/utils.cpp
 )
 
 rclcpp_components_register_node(lanelet2_map_loader_node

map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp

@@ -0,0 +1,77 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP
+#define MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP
+
+#include "lanelet2_extension/io/autoware_multi_osm_parser.hpp"
+#include "map_loader/utils.hpp"
+
+#include <lanelet2_extension/io/autoware_osm_parser.hpp>
+#include <lanelet2_extension/projection/mgrs_projector.hpp>
+#include <lanelet2_extension/utility/message_conversion.hpp>
+#include <lanelet2_extension/utility/utilities.hpp>
+#include <pugixml.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/srv/get_differential_lanelet2_map.hpp"
+#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <autoware_map_msgs/msg/lanelet_map_meta_data.hpp>
+
+#include <lanelet2_core/LaneletMap.h>
+#include <lanelet2_core/geometry/LineString.h>
+#include <lanelet2_io/Io.h>
+#include <lanelet2_projection/UTM.h>
+
+#include <filesystem>
+
+class Lanelet2DifferentialLoaderModule
+{
+  using GetDifferentialLanelet2Map = autoware_map_msgs::srv::GetDifferentialLanelet2Map;
+  using HADMapBin = autoware_auto_mapping_msgs::msg::HADMapBin;
+
+public:
+  explicit Lanelet2DifferentialLoaderModule(
+    rclcpp::Node * node,
+    const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict);
+
+private:
+  rclcpp::Logger logger_;
+  rclcpp::Clock::SharedPtr clock_;
+
+  std::map<std::string, Lanelet2FileMetaData> lanelet2_file_metadata_dict_;
+
+  rclcpp::Service<GetDifferentialLanelet2Map>::SharedPtr get_differential_lanelet2_maps_service_;
+
+  rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
+  rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_debug_;
+
+  rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapMetaData>::SharedPtr
+    pub_lanelet_map_meta_data_;
+
+  bool onServiceGetDifferentialLanelet2Map(
+    GetDifferentialLanelet2Map::Request::SharedPtr req,
+    GetDifferentialLanelet2Map::Response::SharedPtr res);
+
+  bool differentialLanelet2Load(
+    GetDifferentialLanelet2Map::Request::SharedPtr & request,
+    GetDifferentialLanelet2Map::Response::SharedPtr & response);
+
+  autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg() const;
+
+private:
+  // Lanelet Parser Functions
+};
+
+#endif  // MAP_LOADER_LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -15,6 +15,9 @@
 #ifndef MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 #define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_
 
+#include "map_loader/lanelet2_differential_loader_module.hpp"
+#include "map_loader/utils.hpp"
+
 #include <component_interface_specs/map.hpp>
 #include <component_interface_utils/rclcpp.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -47,8 +50,16 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
 
   void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);
 
+  std::unique_ptr<Lanelet2DifferentialLoaderModule> differential_loader_module_;
+
   component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
+
+  std::vector<std::string> getLanelet2Paths(
+    const std::vector<std::string> & lanelet2_paths_or_directory) const;
+  std::map<std::string, Lanelet2FileMetaData> getLanelet2Metadata(
+    const std::string & lanelet2_metadata_path,
+    const std::vector<std::string> & lanelet2_paths) const;
 };
 
 #endif  // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_

map/map_loader/include/map_loader/utils.hpp

@@ -0,0 +1,38 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MAP_LOADER_UTILS_HPP
+#define MAP_LOADER_UTILS_HPP
+
+#include <yaml-cpp/yaml.h>
+
+#include <map>
+#include <string>
+#include <vector>
+
+struct Lanelet2FileMetaData
+{
+  int id;
+  double origin_lat;
+  double origin_lon;
+  std::string mgrs_code;
+};
+
+std::map<std::string, Lanelet2FileMetaData> loadLanelet2Metadata(
+  const std::string & lanelet2_metadata_path);
+std::map<std::string, Lanelet2FileMetaData> replaceWithAbsolutePath(
+  const std::map<std::string, Lanelet2FileMetaData> & lanelet2_metadata_path,
+  const std::vector<std::string> & lanelet2_paths);
+
+#endif  // MAP_LOADER_UTILS_HPP

map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

@@ -0,0 +1,188 @@
+// Copyright 2022 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "map_loader/lanelet2_differential_loader_module.hpp"
+
+#include "map_loader/lanelet2_map_loader_node.hpp"
+
+Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
+  rclcpp::Node * node,
+  const std::map<std::string, Lanelet2FileMetaData> & lanelet2_file_metadata_dict)
+: logger_(node->get_logger()),
+  clock_(node->get_clock()),
+  lanelet2_file_metadata_dict_(lanelet2_file_metadata_dict)
+{
+  // Publish lanelet2 map meta data
+  {
+    pub_lanelet_map_meta_data_ = node->create_publisher<autoware_map_msgs::msg::LaneletMapMetaData>(
+      "/map/lanelet2_map_meta_data", rclcpp::QoS{1}.transient_local());
+
+    const auto msg = getLaneletMapMetaDataMsg();
+
+    pub_lanelet_map_meta_data_->publish(msg);
+  }
+
+  get_differential_lanelet2_maps_service_ = node->create_service<GetDifferentialLanelet2Map>(
+    "/map/get_differential_lanelet2_map",
+    std::bind(
+      &Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map, this,
+      std::placeholders::_1, std::placeholders::_2));
+
+  pub_map_bin_ = node->create_publisher<HADMapBin>(
+    "/map/differential_lanelet2_map_bin", rclcpp::QoS{1}.transient_local());
+  pub_map_bin_debug_ =
+    node->create_publisher<HADMapBin>("/map/vector_map", rclcpp::QoS{1}.transient_local());
+}
+
+bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map(
+  GetDifferentialLanelet2Map::Request::SharedPtr req,
+  GetDifferentialLanelet2Map::Response::SharedPtr res)
+{
+  if (!differentialLanelet2Load(req, res)) {
+    RCLCPP_ERROR(logger_, "Failed to load differential lanelet2 map");
+    return false;
+  }
+  res->header.stamp = clock_->now();
+  res->header.frame_id = "map";
+
+  return true;
+}
+
+bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
+  GetDifferentialLanelet2Map::Request::SharedPtr & request,
+  GetDifferentialLanelet2Map::Response::SharedPtr & response)
+{
+  {
+    std::vector<std::string> lanelet2_paths;
+    for (const auto & path : lanelet2_file_metadata_dict_) {
+      if (!std::filesystem::exists(path.first)) {
+        continue;
+      }
+      lanelet2_paths.push_back(path.first);
+    }
+    if (lanelet2_paths.empty()) {
+      return false;
+    }
+
+    lanelet::ErrorMessages errors{};
+    lanelet::projection::MGRSProjector projector{};
+    lanelet::io_handlers::MultiOsmParser parser(projector, {});
+    lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
+
+    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();
+      }
+    }
+    // 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);
+    }
+    // overwrite centerline
+    lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
+    // convert map to binary message
+    {
+      const auto map_bin_msg =
+        Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
+
+      pub_map_bin_debug_->publish(map_bin_msg);
+    }  // convert map to binary message end
+  }
+
+  {
+    std::vector<std::string> lanelet2_paths;
+    for (const auto & id : request->osm_file_ids) {
+      auto it = std::find_if(
+        lanelet2_file_metadata_dict_.begin(), lanelet2_file_metadata_dict_.end(),
+        [&id](const auto & file) { return file.second.id == id; });
+      if (it == lanelet2_file_metadata_dict_.end()) {
+        continue;
+      }
+      if (!std::filesystem::exists(it->first)) {
+        continue;
+      }
+      lanelet2_paths.push_back(it->first);
+    }
+    if (lanelet2_paths.empty()) {
+      return false;
+    }
+
+    lanelet::ErrorMessages errors{};
+    lanelet::projection::MGRSProjector projector{};
+    lanelet::io_handlers::MultiOsmParser parser(projector);
+    lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
+
+    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();
+      }
+    }
+    // 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);
+    }
+    // overwrite centerline
+    lanelet::utils::overwriteLaneletsCenterline(map, 5.0, false);
+    // convert map to binary message
+    {
+      const auto map_bin_msg =
+        Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
+
+      pub_map_bin_->publish(map_bin_msg);
+      response->differential_map = map_bin_msg;
+    }
+  }
+
+  return true;
+}
+
+autoware_map_msgs::msg::LaneletMapMetaData
+Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg() const
+{
+  autoware_map_msgs::msg::LaneletMapMetaData metadata;
+  for (const auto & file : lanelet2_file_metadata_dict_) {
+    std::cout << "----------------" << std::endl;
+    std::cout << file.first << std::endl;
+    std::cout << file.second.id << std::endl;
+
+    autoware_map_msgs::msg::LaneletMapTileMetaData tile_msg;
+    tile_msg.mgrs_code = file.second.mgrs_code;
+    tile_msg.origin_lat = file.second.origin_lat;
+    tile_msg.origin_lon = file.second.origin_lon;
+
+    autoware_map_msgs::msg::LaneletMapTileMetaDataWithID tile_msg_with_id;
+    tile_msg_with_id.metadata = tile_msg;
+    tile_msg_with_id.tile_id = file.second.id;
+
+    metadata.metadata_list.push_back(tile_msg_with_id);
+  }
+  metadata.header.frame_id = "map";
+  metadata.header.stamp = clock_->now();
+
+  return metadata;
+}