Add projector types

Signed-off-by: Barış Zeren <bzeren1819@gmail.com>

Author: StepTurtle

launch/tier4_map_launch/launch/map.launch.xml

@@ -16,14 +16,14 @@
   <arg name="use_intra_process" default="false"/>
 
   <!-- select container type -->
-  <arg name="use_multithread" default="false"/>
+  <arg name="use_multithread" default="true"/>
   <let name="container_type" value="component_container" unless="$(var use_multithread)"/>
   <let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
 
   <group>
     <push-ros-namespace namespace="map"/>
 
-    <node_container pkg="rclcpp_components" exec="component_container_mt" name="map_container" namespace="" output="screen">
+    <node_container pkg="rclcpp_components" exec="$(var container_type)" name="map_container" namespace="" output="screen">
       <composable_node pkg="map_loader" plugin="PointCloudMapLoaderNode" name="pointcloud_map_loader">
         <param from="$(var pointcloud_map_loader_param_path)"/>
         <param name="pcd_paths_or_directory" value="[$(var pointcloud_map_path)]"/>

map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml

@@ -1,5 +1,5 @@
 /**:
   ros__parameters:
-    dynamic_map_loading_grid_size: 5.0         # [m]
-    dynamic_map_loading_update_distance: 10.0  # [m]
-    dynamic_map_loading_map_radius: 15.0       # [m]
+    dynamic_map_loading_grid_size: 50.0         # [m]
+    dynamic_map_loading_update_distance: 100.0  # [m]
+    dynamic_map_loading_map_radius: 150.0       # [m]

map/autoware_dynamic_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp

@@ -5,6 +5,7 @@
 
 #include <component_interface_specs/map.hpp>
 #include <component_interface_utils/rclcpp.hpp>
+#include <geography_utils/lanelet2_projector.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
 #include <rclcpp/rclcpp.hpp>
 
@@ -13,6 +14,7 @@
 #include "autoware_map_msgs/srv/get_differential_lanelet2_map.hpp"
 #include <geometry_msgs/msg/point.hpp>
 #include <nav_msgs/msg/odometry.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
 
 namespace autoware
 {
@@ -45,6 +47,9 @@ class DynamicLaneletProviderNode : public rclcpp::Node
 
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
 
+  component_interface_utils::Subscription<map_interface::MapProjectorInfo>::SharedPtr
+    sub_map_projector_info_;
+
   rclcpp::Subscription<autoware_map_msgs::msg::LaneletMapMetaData>::SharedPtr
     lanelet_map_meta_data_sub_;
 
@@ -63,6 +68,8 @@ class DynamicLaneletProviderNode : public rclcpp::Node
   const double dynamic_map_loading_map_radius_;
 
   std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;
+
+  std::optional<tier4_map_msgs::msg::MapProjectorInfo> projector_info_;
 };
 }  // namespace dynamic_lanelet_provider
 }  // namespace autoware

map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp

@@ -43,11 +43,13 @@ double norm_xy(const T & p1, const U & p2)
 DynamicLaneletProviderNode::DynamicLaneletProviderNode(const rclcpp::NodeOptions & options)
 : Node("dynamic_lanelet_provider", options),
   map_frame_("map"),
-  dynamic_map_loading_grid_size_(50),
-  dynamic_map_loading_update_distance_(dynamic_map_loading_grid_size_ * sqrt(2)),
-  dynamic_map_loading_map_radius_(dynamic_map_loading_grid_size_ * sqrt(5))
+  dynamic_map_loading_grid_size_(declare_parameter<double>("dynamic_map_loading_grid_size")),
+  dynamic_map_loading_update_distance_(
+    declare_parameter<double>("dynamic_map_loading_update_distance")),
+  dynamic_map_loading_map_radius_(declare_parameter<double>("dynamic_map_loading_map_radius"))
 {
-  client_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  client_callback_group_ =
+    this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
   timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
 
   dynamic_map_pub_ = this->create_publisher<autoware_auto_mapping_msgs::msg::HADMapBin>(
@@ -60,6 +62,13 @@ DynamicLaneletProviderNode::DynamicLaneletProviderNode(const rclcpp::NodeOptions
     "/localization/kinematic_state", 100,
     std::bind(&DynamicLaneletProviderNode::onOdometry, this, std::placeholders::_1));
 
+  const auto adaptor = component_interface_utils::NodeAdaptor(this);
+  adaptor.init_sub(
+    sub_map_projector_info_,
+    [this](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
+      projector_info_ = *msg;
+    });
+
   lanelet_map_meta_data_sub_ =
     this->create_subscription<autoware_map_msgs::msg::LaneletMapMetaData>(
       "/map/lanelet_map_meta_data", rclcpp::QoS{1}.transient_local(),
@@ -92,6 +101,10 @@ void DynamicLaneletProviderNode::mapUpdateTimerCallback()
     return;
   }
 
+  if (projector_info_ == std::nullopt || lanelet_map_meta_data_list_.empty()) {
+    RCLCPP_ERROR_ONCE(get_logger(), "Check your lanelet map metadata and projector info.");
+  }
+
   if (should_update_map()) {
     RCLCPP_INFO(get_logger(), "Start updating lanelet map (timer callback)");
 
@@ -102,25 +115,31 @@ void DynamicLaneletProviderNode::mapUpdateTimerCallback()
 
 void DynamicLaneletProviderNode::updateMap(const geometry_msgs::msg::Point & pose)
 {
-  lanelet::projection::MGRSProjector projector;
-
   std::vector<int> cache_ids;
-  for (const auto & metadata : lanelet_map_meta_data_list_) {
-    lanelet::GPSPoint gps_point;
-    gps_point.lat = metadata.origin_lat;
-    gps_point.lon = metadata.origin_lon;
-    gps_point.ele = 0;
-
-    lanelet::BasicPoint3d point = projector.forward(gps_point);
-
-    double distance = norm_xy(point, pose);
-    if (distance < dynamic_map_loading_map_radius_) {
-      cache_ids.push_back(metadata.id);
+  if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
+    std::unique_ptr<lanelet::Projector> projector =
+      geography_utils::get_lanelet2_projector(projector_info_.value());
+
+    for (const auto & metadata : lanelet_map_meta_data_list_) {
+      lanelet::GPSPoint gps_point;
+      gps_point.lat = metadata.origin_lat;
+      gps_point.lon = metadata.origin_lon;
+      gps_point.ele = 0;
+
+      lanelet::BasicPoint3d point = projector->forward(gps_point);
+
+      double distance = norm_xy(point, pose);
+      if (distance < dynamic_map_loading_map_radius_) {
+        cache_ids.push_back(metadata.id);
+      }
     }
+  } else {
+    // TODO: cannot reach LocalProjector here
   }
 
-  for (const auto & id : cache_ids) {
-    std::cout << "id: " << id << std::endl;
+  if (cache_ids.empty()) {
+    RCLCPP_ERROR(get_logger(), "No lanelet map is found in the radius.");
+    return;
   }
 
   auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialLanelet2Map::Request>();

map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp

@@ -18,6 +18,9 @@
 #include "lanelet2_extension/io/autoware_multi_osm_parser.hpp"
 #include "map_loader/utils.hpp"
 
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <geography_utils/lanelet2_projector.hpp>
 #include <lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
 #include <lanelet2_extension/utility/message_conversion.hpp>
@@ -28,6 +31,7 @@
 #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 <tier4_map_msgs/msg/map_projector_info.hpp>
 
 #include <lanelet2_core/LaneletMap.h>
 #include <lanelet2_core/geometry/LineString.h>
@@ -57,6 +61,11 @@ class Lanelet2DifferentialLoaderModule
   rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapMetaData>::SharedPtr
     pub_lanelet_map_meta_data_;
 
+  component_interface_utils::Subscription<map_interface::MapProjectorInfo>::SharedPtr
+    sub_map_projector_info_;
+
+  std::optional<tier4_map_msgs::msg::MapProjectorInfo> projector_info_;
+
   bool onServiceGetDifferentialLanelet2Map(
     GetDifferentialLanelet2Map::Request::SharedPtr req,
     GetDifferentialLanelet2Map::Response::SharedPtr res);

map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp

@@ -14,6 +14,7 @@
 
 #include "map_loader/lanelet2_differential_loader_module.hpp"
 
+#include "lanelet2_local_projector.hpp"
 #include "map_loader/lanelet2_map_loader_node.hpp"
 
 Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
@@ -33,6 +34,13 @@ Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule(
     pub_lanelet_map_meta_data_->publish(msg);
   }
 
+  const auto adaptor = component_interface_utils::NodeAdaptor(node);
+  adaptor.init_sub(
+    sub_map_projector_info_,
+    [this](const map_interface::MapProjectorInfo::Message::ConstSharedPtr msg) {
+      projector_info_ = *msg;
+    });
+
   get_differential_lanelet2_maps_service_ = node->create_service<GetDifferentialLanelet2Map>(
     "/map/get_differential_lanelet_map",
     std::bind(
@@ -75,39 +83,61 @@ bool Lanelet2DifferentialLoaderModule::differentialLanelet2Load(
     return false;
   }
 
-  // create parser and load map
-  lanelet::ErrorMessages errors{};
-  lanelet::projection::MGRSProjector projector{};
-  lanelet::io_handlers::MultiOsmParser parser(projector);
-  lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
+  if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) {
+    std::unique_ptr<lanelet::Projector> projector =
+      geography_utils::get_lanelet2_projector(projector_info_.value());
+
+    lanelet::ErrorMessages errors{};
+    lanelet::io_handlers::MultiOsmParser parser(*projector);
+    lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
+
+    if (!errors.empty()) {
+      for (const auto & error : errors) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
+      }
+    }
+
+    const auto map_bin_msg =
+      Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now());
 
-  for (lanelet::Point3d point : map->pointLayer) {
-    if (point.hasAttribute("local_x")) {
-      point.x() = point.attribute("local_x").asDouble().value();
+    response->differential_map = map_bin_msg;
+  } else {
+    const lanelet::projection::LocalProjector projector;
+    lanelet::ErrorMessages errors{};
+    lanelet::io_handlers::MultiOsmParser parser(projector);
+    lanelet::LaneletMapPtr map = parser.parse(lanelet2_paths, errors);
+
+    if (!errors.empty()) {
+      for (const auto & error : errors) {
+        RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error);
+      }
     }
-    if (point.hasAttribute("local_y")) {
-      point.y() = point.attribute("local_y").asDouble().value();
+
+    // overwrite local_x, local_y
+    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);
-  }
+    // 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());
 
     response->differential_map = map_bin_msg;
   }
+
   return true;
 }