feat(map): add differential lanelet loading

common/component_interface_specs/include/component_interface_specs/map.hpp

@@ -17,6 +17,7 @@
 
 #include <rclcpp/qos.hpp>
 
+#include <autoware_map_msgs/msg/lanelet_map_meta_data.hpp>
 #include <tier4_map_msgs/msg/map_projector_info.hpp>
 
 namespace map_interface
@@ -31,6 +32,15 @@ struct MapProjectorInfo
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
 };
 
+struct LaneletMapMetaData
+{
+  using Message = autoware_map_msgs::msg::LaneletMapMetaData;
+  static constexpr char name[] = "/map/lanelet_map_meta_data";
+  static constexpr size_t depth = 1;
+  static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+};
+
 }  // namespace map_interface
 
 #endif  // COMPONENT_INTERFACE_SPECS__MAP_HPP_

common/component_interface_specs/package.xml

@@ -12,6 +12,7 @@
   <buildtool_depend>autoware_cmake</buildtool_depend>
 
   <depend>autoware_adapi_v1_msgs</depend>
+  <depend>autoware_map_msgs</depend>
   <depend>autoware_perception_msgs</depend>
   <depend>autoware_planning_msgs</depend>
   <depend>autoware_vehicle_msgs</depend>

common/component_interface_specs/test/test_map.cpp

@@ -25,4 +25,13 @@ TEST(map, interface)
     EXPECT_EQ(map_projector.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
     EXPECT_EQ(map_projector.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
   }
+
+  {
+    using map_interface::LaneletMapMetaData;
+    LaneletMapMetaData lanelet_metadata;
+    size_t depth = 1;
+    EXPECT_EQ(lanelet_metadata.depth, depth);
+    EXPECT_EQ(lanelet_metadata.reliability, RMW_QOS_POLICY_RELIABILITY_RELIABLE);
+    EXPECT_EQ(lanelet_metadata.durability, RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL);
+  }
 }

launch/tier4_map_launch/launch/map.launch.xml

@@ -3,19 +3,25 @@
   <arg name="pointcloud_map_path"/>
   <arg name="pointcloud_map_metadata_path"/>
   <arg name="lanelet2_map_path"/>
+  <arg name="lanelet2_map_metadata_path"/>
   <arg name="map_projector_info_path"/>
 
   <!-- Parameter files -->
   <arg name="pointcloud_map_loader_param_path"/>
   <arg name="lanelet2_map_loader_param_path"/>
+  <arg name="dynamic_lanelet_provider_param_path"/>
   <arg name="map_tf_generator_param_path"/>
   <arg name="map_projection_loader_param_path"/>
 
+  <!-- Dynamic lanelet loading -->
+  <arg name="enable_differential_map_loading"/>
+
   <!-- whether use intra-process -->
   <arg name="use_intra_process" default="false"/>
 
   <!-- select container type -->
-  <arg name="use_multithread" default="false"/>
+  <arg name="use_multithread" default="false" unless="$(var enable_differential_map_loading)"/>
+  <arg name="use_multithread" default="true" if="$(var enable_differential_map_loading)"/>
   <let name="container_type" value="component_container" unless="$(var use_multithread)"/>
   <let name="container_type" value="component_container_mt" if="$(var use_multithread)"/>
 
@@ -38,7 +44,23 @@
       <composable_node pkg="map_loader" plugin="Lanelet2MapLoaderNode" name="lanelet2_map_loader">
         <param from="$(var lanelet2_map_loader_param_path)"/>
         <param name="lanelet2_map_path" value="$(var lanelet2_map_path)"/>
+        <param name="lanelet2_map_metadata_path" value="$(var lanelet2_map_metadata_path)"/>
+        <param name="enable_differential_map_loading" value="$(var enable_differential_map_loading)"/>
         <remap from="output/lanelet2_map" to="vector_map"/>
+        <remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
+        <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+      </composable_node>
+
+      <composable_node
+        pkg="autoware_dynamic_lanelet_provider"
+        plugin="autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
+        name="dynamic_lanelet_provider"
+        if="$(var enable_differential_map_loading)"
+      >
+        <param from="$(var dynamic_lanelet_provider_param_path)"/>
+        <remap from="input/odometry" to="/localization/kinematic_state"/>
+        <remap from="output/lanelet2_map" to="dynamic_vector_map"/>
+        <remap from="service/get_differential_lanelet_map" to="/map/get_differential_lanelet_map"/>
         <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
       </composable_node>
 
@@ -48,6 +70,12 @@
         <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
       </composable_node>
 
+      <composable_node pkg="map_loader" plugin="Lanelet2MapVisualizationNode" name="dynamic_lanelet2_map_visualization" if="$(var enable_differential_map_loading)">
+        <remap from="input/lanelet2_map" to="/map/dynamic_vector_map"/>
+        <remap from="output/lanelet2_map_marker" to="/map/dynamic_vector_map_marker"/>
+        <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>
+      </composable_node>
+
       <composable_node pkg="autoware_map_tf_generator" plugin="autoware::map_tf_generator::VectorMapTFGeneratorNode" name="vector_map_tf_generator">
         <param from="$(var map_tf_generator_param_path)"/>
         <extra_arg name="use_intra_process_comms" value="$(var use_intra_process)"/>

map/autoware_dynamic_lanelet_provider/CMakeLists.txt

@@ -0,0 +1,19 @@
+cmake_minimum_required(VERSION 3.14)
+project(autoware_dynamic_lanelet_provider)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_library(dynamic_lanelet_provider_node SHARED
+  src/dynamic_lanelet_provider.cpp
+)
+
+rclcpp_components_register_node(dynamic_lanelet_provider_node
+  PLUGIN "autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode"
+  EXECUTABLE dynamic_lanelet_provider
+)
+
+ament_auto_package(INSTALL_TO_SHARE
+        launch
+        config
+)

map/autoware_dynamic_lanelet_provider/README.md

@@ -0,0 +1,37 @@
+# autoware_dynamic_lanelet_provider
+
+## Purpose
+
+This package aims to provide a dynamic Lanelet2 map to the other Autoware nodes.
+The dynamic Lanelet2 map is a Lanelet2 map that is updated in real-time based
+on the current odometry position of the vehicle.
+
+To use this package, you need to provide a divided Lanelet2 maps and a metadata file.
+You should check
+the [lanelet2_map_loader documentation](https://autowarefoundation.github.io/autoware.universe/main/map/map_loader/#lanelet2_map_loader)
+for more information about the divided lanelet map and the metadata file.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                         | Type                                       | Description                     |
+| ---------------------------- | ------------------------------------------ | ------------------------------- |
+| `~/input/odometry`           | `nav_msgs::msg::Odometry`                  | ego vehicle odometry            |
+| `/map/lanelet_map_meta_data` | autoware_map_msgs::msg::LaneletMapMetaData | metadata info for lanelet2 maps |
+
+### Output
+
+| Name                  | Type                                    | Description                |
+| --------------------- | --------------------------------------- | -------------------------- |
+| `output/lanelet2_map` | `autoware_map_msgs::msg::LaneletMapBin` | dynamic Lanelet2 map topic |
+
+### Client
+
+| Name                                   | Type                                             | Description                                |
+| -------------------------------------- | ------------------------------------------------ | ------------------------------------------ |
+| `service/get_differential_lanelet_map` | `autoware_map_msgs::srv::GetSelectedLanelet2Map` | service to load differential Lanelet2 maps |
+
+## Parameters
+
+{{ json_to_markdown("map/autoware_dynamic_lanelet_provider/schema/dynamic_lanelet_provider.schema.json") }}

map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml

@@ -0,0 +1,4 @@
+/**:
+  ros__parameters:
+    dynamic_map_loading_update_distance: 100.0  # [m]
+    dynamic_map_loading_map_radius: 200.0       # [m]

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

@@ -0,0 +1,85 @@
+// Copyright 2024 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 AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
+#define AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_
+
+#include <component_interface_specs/map.hpp>
+#include <component_interface_utils/rclcpp.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "autoware_map_msgs/msg/lanelet_map_meta_data.hpp"
+#include "autoware_map_msgs/srv/get_selected_lanelet2_map.hpp"
+#include <geometry_msgs/msg/point.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
+
+#include <optional>
+#include <string>
+#include <vector>
+
+namespace autoware
+{
+namespace dynamic_lanelet_provider
+{
+
+struct Lanelet2FileMetaData
+{
+  std::string id;
+  double min_x;
+  double max_x;
+  double min_y;
+  double max_y;
+};
+
+class DynamicLaneletProviderNode : public rclcpp::Node
+{
+public:
+  explicit DynamicLaneletProviderNode(const rclcpp::NodeOptions & options);
+
+private:
+  void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg);
+  void mapUpdateTimerCallback();
+
+  void updateMap(const geometry_msgs::msg::Point & pose);
+  bool should_update_map() const;
+
+  rclcpp::Publisher<autoware_map_msgs::msg::LaneletMapBin>::SharedPtr dynamic_map_pub_;
+
+  rclcpp::Client<autoware_map_msgs::srv::GetSelectedLanelet2Map>::SharedPtr map_loader_client_;
+
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_sub_;
+
+  component_interface_utils::Subscription<map_interface::LaneletMapMetaData>::SharedPtr
+    lanelet_map_meta_data_sub_;
+
+  rclcpp::TimerBase::SharedPtr map_update_timer_;
+
+  rclcpp::CallbackGroup::SharedPtr client_callback_group_;
+  rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
+
+  std::string map_frame_;
+
+  std::optional<geometry_msgs::msg::Point> last_update_position_ = std::nullopt;
+  std::optional<geometry_msgs::msg::Point> current_position_ = std::nullopt;
+
+  const double dynamic_map_loading_update_distance_;
+  const double dynamic_map_loading_map_radius_;
+
+  std::vector<Lanelet2FileMetaData> lanelet_map_meta_data_list_;
+};
+}  // namespace dynamic_lanelet_provider
+}  // namespace autoware
+
+#endif  // AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_

map/autoware_dynamic_lanelet_provider/launch/dynamic_lanelet_provider.launch.xml

@@ -0,0 +1,7 @@
+<launch>
+  <arg name="dynamic_lanelet_provider_param_path" default="$(find-pkg-share autoware_dynamic_lanelet_provider)/config/dynamic_lanelet_provider.param.yaml"/>
+
+  <node pkg="autoware_dynamic_lanelet_provider" exec="dynamic_lanelet_provider_node" name="dynamic_lanelet_provider_node">
+    <param from="$(var dynamic_lanelet_provider_param_path)"/>
+  </node>
+</launch>

map/autoware_dynamic_lanelet_provider/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_dynamic_lanelet_provider</name>
+  <version>0.1.0</version>
+  <description>Dynamic map provider package</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Ryu Yamamoto</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">NGUYEN Viet Anh</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Barış Zeren</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="[email protected]">Barış Zeren</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <depend>autoware_geography_utils</depend>
+  <depend>autoware_map_msgs</depend>
+  <depend>component_interface_specs</depend>
+  <depend>component_interface_utils</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

map/autoware_dynamic_lanelet_provider/schema/dynamic_lanelet_provider.schema.json

@@ -0,0 +1,38 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Parameters for autoware_dynamic_lanelet_provider",
+  "type": "object",
+  "definitions": {
+    "autoware_dynamic_lanelet_provider": {
+      "type": "object",
+      "properties": {
+        "dynamic_map_loading_update_distance": {
+          "type": "number",
+          "description": "The distance to update the map [m]",
+          "default": "100.0"
+        },
+        "dynamic_map_loading_map_radius": {
+          "type": "number",
+          "description": "The radius for the map to be loaded [m]",
+          "default": "200.0"
+        }
+      },
+      "required": ["dynamic_map_loading_update_distance", "dynamic_map_loading_map_radius"],
+      "additionalProperties": false
+    }
+  },
+  "properties": {
+    "/**": {
+      "type": "object",
+      "properties": {
+        "ros__parameters": {
+          "$ref": "#/definitions/autoware_dynamic_lanelet_provider"
+        }
+      },
+      "required": ["ros__parameters"],
+      "additionalProperties": false
+    }
+  },
+  "required": ["/**"],
+  "additionalProperties": false
+}