diff --git a/common/component_interface_specs/include/component_interface_specs/map.hpp b/common/component_interface_specs/include/component_interface_specs/map.hpp index dc162d4a7267a..d3681d79b39cc 100644 --- a/common/component_interface_specs/include/component_interface_specs/map.hpp +++ b/common/component_interface_specs/include/component_interface_specs/map.hpp @@ -17,6 +17,7 @@ #include +#include #include 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_ diff --git a/common/component_interface_specs/package.xml b/common/component_interface_specs/package.xml index fa57bb1641eed..fdb7b62c185ec 100644 --- a/common/component_interface_specs/package.xml +++ b/common/component_interface_specs/package.xml @@ -12,6 +12,7 @@ autoware_cmake autoware_adapi_v1_msgs + autoware_map_msgs autoware_perception_msgs autoware_planning_msgs autoware_vehicle_msgs diff --git a/common/component_interface_specs/test/test_map.cpp b/common/component_interface_specs/test/test_map.cpp index a65f2cb7120d1..d61505d7efead 100644 --- a/common/component_interface_specs/test/test_map.cpp +++ b/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); + } } diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index bd97963bd1268..d50b54f293ce4 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -3,19 +3,25 @@ + + + + + - + + @@ -38,7 +44,23 @@ + + + + + + + + + + + @@ -48,6 +70,12 @@ + + + + + + diff --git a/map/autoware_dynamic_lanelet_provider/CMakeLists.txt b/map/autoware_dynamic_lanelet_provider/CMakeLists.txt new file mode 100644 index 0000000000000..aa25a9e2430d5 --- /dev/null +++ b/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 +) diff --git a/map/autoware_dynamic_lanelet_provider/README.md b/map/autoware_dynamic_lanelet_provider/README.md new file mode 100644 index 0000000000000..2273bfa249530 --- /dev/null +++ b/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") }} diff --git a/map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml b/map/autoware_dynamic_lanelet_provider/config/dynamic_lanelet_provider.param.yaml new file mode 100644 index 0000000000000..b4f71e3b9fdde --- /dev/null +++ b/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] diff --git a/map/autoware_dynamic_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp b/map/autoware_dynamic_lanelet_provider/include/autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp new file mode 100644 index 0000000000000..8ca71b96020b6 --- /dev/null +++ b/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 +#include +#include + +#include "autoware_map_msgs/msg/lanelet_map_meta_data.hpp" +#include "autoware_map_msgs/srv/get_selected_lanelet2_map.hpp" +#include +#include +#include + +#include +#include +#include + +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::SharedPtr dynamic_map_pub_; + + rclcpp::Client::SharedPtr map_loader_client_; + + rclcpp::Subscription::SharedPtr odometry_sub_; + + component_interface_utils::Subscription::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 last_update_position_ = std::nullopt; + std::optional current_position_ = std::nullopt; + + const double dynamic_map_loading_update_distance_; + const double dynamic_map_loading_map_radius_; + + std::vector lanelet_map_meta_data_list_; +}; +} // namespace dynamic_lanelet_provider +} // namespace autoware + +#endif // AUTOWARE__DYNAMIC_LANELET_PROVIDER__DYNAMIC_LANELET_PROVIDER_HPP_ diff --git a/map/autoware_dynamic_lanelet_provider/launch/dynamic_lanelet_provider.launch.xml b/map/autoware_dynamic_lanelet_provider/launch/dynamic_lanelet_provider.launch.xml new file mode 100644 index 0000000000000..ea8cd938a245b --- /dev/null +++ b/map/autoware_dynamic_lanelet_provider/launch/dynamic_lanelet_provider.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/map/autoware_dynamic_lanelet_provider/package.xml b/map/autoware_dynamic_lanelet_provider/package.xml new file mode 100644 index 0000000000000..7e73bc85d74de --- /dev/null +++ b/map/autoware_dynamic_lanelet_provider/package.xml @@ -0,0 +1,28 @@ + + + + autoware_dynamic_lanelet_provider + 0.1.0 + Dynamic map provider package + Yamato Ando + Ryu Yamamoto + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Barış Zeren + Apache License 2.0 + Barış Zeren + + ament_cmake_auto + autoware_cmake + + autoware_geography_utils + autoware_map_msgs + component_interface_specs + component_interface_utils + + + ament_cmake + + diff --git a/map/autoware_dynamic_lanelet_provider/schema/dynamic_lanelet_provider.schema.json b/map/autoware_dynamic_lanelet_provider/schema/dynamic_lanelet_provider.schema.json new file mode 100644 index 0000000000000..098e7563e401a --- /dev/null +++ b/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 +} diff --git a/map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp b/map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp new file mode 100644 index 0000000000000..2b076da9430d0 --- /dev/null +++ b/map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp @@ -0,0 +1,209 @@ +// 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. + +#include "autoware/dynamic_lanelet_provider/dynamic_lanelet_provider.hpp" + +namespace autoware +{ +namespace dynamic_lanelet_provider +{ + +// Define a helper function to get x and y +template +auto getX(const T & point) -> decltype(point.x) +{ + return point.x; +} + +template +auto getY(const T & point) -> decltype(point.y) +{ + return point.y; +} + +// Define a helper function to get x() and y() +template +auto getX(const T & point) -> decltype(point.x()) +{ + return point.x(); +} + +template +auto getY(const T & point) -> decltype(point.y()) +{ + return point.y(); +} + +template +double norm_xy(const T & p1, const U & p2) +{ + double dx = getX(p1) - getX(p2); + double dy = getY(p1) - getY(p2); + return std::hypot(dx, dy); +} + +template +bool is_inside_region( + const double & min_x, const double & max_x, const double & min_y, const double & max_y, + const T & point) +{ + return min_x <= getX(point) && getX(point) <= max_x && min_y <= getY(point) && + getY(point) <= max_y; +} + +DynamicLaneletProviderNode::DynamicLaneletProviderNode(const rclcpp::NodeOptions & options) +: Node("dynamic_lanelet_provider", options), + map_frame_("map"), + dynamic_map_loading_update_distance_( + declare_parameter("dynamic_map_loading_update_distance")), + dynamic_map_loading_map_radius_(declare_parameter("dynamic_map_loading_map_radius")) +{ + 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( + "output/lanelet2_map", rclcpp::QoS{1}.transient_local()); + + map_loader_client_ = this->create_client( + "service/get_differential_lanelet_map", rmw_qos_profile_services_default, + client_callback_group_); + + odometry_sub_ = this->create_subscription( + "input/odometry", 1, + std::bind(&DynamicLaneletProviderNode::onOdometry, this, std::placeholders::_1)); + + const auto metadata_adaptor = component_interface_utils::NodeAdaptor(this); + metadata_adaptor.init_sub( + lanelet_map_meta_data_sub_, + [this](const autoware_map_msgs::msg::LaneletMapMetaData::SharedPtr msg) { + for (const auto & data : msg->metadata_list) { + Lanelet2FileMetaData metadata; + metadata.id = data.cell_id; + metadata.min_x = data.min_x; + metadata.max_x = data.max_x; + metadata.min_y = data.min_y; + metadata.max_y = data.max_y; + lanelet_map_meta_data_list_.push_back(metadata); + } + }); + + double map_update_dt = 1.0; + auto period_ns = std::chrono::duration_cast( + std::chrono::duration(map_update_dt)); + map_update_timer_ = rclcpp::create_timer( + this, get_clock(), period_ns, + std::bind(&DynamicLaneletProviderNode::mapUpdateTimerCallback, this), timer_callback_group_); +} + +void DynamicLaneletProviderNode::mapUpdateTimerCallback() +{ + if (current_position_ == std::nullopt) { + RCLCPP_ERROR_STREAM_THROTTLE( + get_logger(), *get_clock(), 1, + "Cannot find the reference position for lanelet map update. Please check if the EKF odometry " + "is provided to behavior planner map update module."); + return; + } + + if (lanelet_map_meta_data_list_.empty()) { + RCLCPP_ERROR_ONCE(get_logger(), "Check your lanelet map metadata and projector info."); + return; + } + + if (should_update_map()) { + RCLCPP_INFO(get_logger(), "Start updating lanelet map (timer callback)"); + + last_update_position_ = current_position_; + updateMap(current_position_.value()); + } +} + +void DynamicLaneletProviderNode::updateMap(const geometry_msgs::msg::Point & pose) +{ + std::vector cache_ids; + for (const auto & metadata : lanelet_map_meta_data_list_) { + geometry_msgs::msg::Point point; + point.x = (metadata.min_x + metadata.max_x) / 2; + point.y = (metadata.min_y + metadata.max_y) / 2; + + if (is_inside_region(metadata.min_x, metadata.max_x, metadata.min_y, metadata.max_y, pose)) { + cache_ids.push_back(metadata.id); + continue; + } + + double distance = norm_xy(point, pose); + if (distance < dynamic_map_loading_map_radius_) { + cache_ids.push_back(metadata.id); + } + } + + if (cache_ids.empty()) { + RCLCPP_ERROR(get_logger(), "No lanelet map is found in the radius."); + return; + } + + auto request = std::make_shared(); + request->cell_ids.insert(request->cell_ids.end(), cache_ids.begin(), cache_ids.end()); + + while (!map_loader_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO(get_logger(), "Waiting for lanelet loader service"); + } + + auto result{map_loader_client_->async_send_request( + request, [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + switch (status) { + case std::future_status::ready: + RCLCPP_INFO(get_logger(), "The future status is (ready)."); + break; + case std::future_status::timeout: + RCLCPP_INFO(get_logger(), "The future status is (timed out)."); + break; + case std::future_status::deferred: + RCLCPP_INFO(get_logger(), "The future status is (deferred)."); + break; + } + RCLCPP_INFO(get_logger(), "waiting response from lanelet loader service."); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + + dynamic_map_pub_->publish(result.get()->lanelet2_cells); +} + +void DynamicLaneletProviderNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr & msg) +{ + current_position_ = msg->pose.pose.position; +} + +bool DynamicLaneletProviderNode::should_update_map() const +{ + if (last_update_position_ == std::nullopt) { + return true; + } + + double distance = norm_xy(current_position_.value(), last_update_position_.value()); + return distance > dynamic_map_loading_update_distance_; +} + +} // namespace dynamic_lanelet_provider +} // namespace autoware + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware::dynamic_lanelet_provider::DynamicLaneletProviderNode) diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index 115f7e5b17464..87535edfa896d 100644 --- a/map/map_loader/CMakeLists.txt +++ b/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 diff --git a/map/map_loader/README.md b/map/map_loader/README.md index bc3eb80d80339..f427a515729d7 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -130,9 +130,74 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co ### Feature -lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_map_msgs/LaneletMapBin message. +`lanelet2_map_loader` loads Lanelet2 file(s) and publishes the map data as `autoware_map_msgs/LaneletMapBin` message. The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`. Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tier4/tier4_autoware_msgs/blob/tier4/universe/tier4_map_msgs/msg/MapProjectorInfo.msg) for supported projector types. +lanelet2_map_loader provides Lanelet2 maps in two different configurations: + +- It loads single Lanelet2 file and publishes the map data as `autoware_map_msgs/LaneletMapBin` message. +- It loads multiple Lanelet2 files differentially via ROS 2 service. + +NOTE: **If you work on large scale area, we recommend to use differential loading feature to avoid the time-consuming +in various nodes that use the Lanelet2 map data.** + +### Prerequisites + +#### Prerequisites on Lanelet2 map file(s) + +You may provide either a single `.osm` file or multiple `.osm` files. If you are using multiple OSM file, +it MUST obey the following rules: + +1. **The `.osm` files should all be within the same MGRS grid**. The system does not support loading multiple `.osm` files from different MGRS grids. +2. **It must be divided by straight lines parallel to the x-axis and y-axis**. The system does not support division by diagonal lines or curved lines. +3. **Metadata file should also be provided.** The metadata structure description is provided below. + +#### Metadata structure + +The metadata should look like this: + +```yaml +x_resolution: 20.0 +y_resolution: 20.0 +A.osm: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520 +B.osm: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520 +C.osm: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540 +D.osm: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540 +``` + +where, + +- `x_resolution` and `y_resolution` are the resolution of the map in meters. +- `A.osm`, `B.osm`, etc, are the names of OSM files. +- List such as `[1200, 2500]` are the values indicate that for this OSM file, x coordinates are between 1200 and 1220 (`x_resolution` + `x_coordinate`) and y coordinates are between 2500 and 2520 (`y_resolution` + `y_coordinate`). + +You may use [lanelet2_map_tile_generator](https://github.com/leo-drive/lanelet2-map-tile-generator) from LeoDrive for +dividing Lanelet2 map as well as generating the compatible metadata.yaml. + +#### Directory structure of these files + +If you only have one Lanelet2 map, Autoware will assume the following directory structure by default. + +```bash +sample-map-rosbag +├── lanelet2_map.osm +├── pointcloud_map.pcd +``` + +If you have multiple Lanelet2 maps, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple Lanelet2 map files. + +```bash +sample-map-rosbag +├── pointcloud_map.pcd +├── lanelet2_map.osm +│ ├── A.osm +│ ├── B.osm +│ ├── C.osm +│ └── ... +├── map_projector_info.yaml +├── lanelet2_map_metadata.yaml +└── ... +``` ### How to run @@ -144,8 +209,20 @@ Please see [tier4_autoware_msgs/msg/MapProjectorInfo.msg](https://github.com/tie ### Published Topics +If you use single Lanelet2 map file: + - ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map +If you use multiple Lanelet2 map files: + +- ~output/lanelet_map_meta_data (autoware_map_msgs/msg/LaneletMapMetaData) : Metadata of Lanelet2 Map + +### Services + +If you use multiple Lanelet2 map files: + +- ~service/get_differential_lanelet_map (autoware_map_msgs/srv/GetSelectedLanelet2Map) : Differential loading of Lanelet2 Map + ### Parameters {{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml index 48d392a1b8e66..2fe25e8568dae 100755 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/map_loader/config/lanelet2_map_loader.param.yaml @@ -1,6 +1,12 @@ /**: ros__parameters: + enable_differential_map_loading: $(var enable_differential_map_loading) # flag to enable loading of differential map allow_unsupported_version: true # flag to load unsupported format_version anyway. If true, just prints warning. center_line_resolution: 5.0 # [m] use_waypoints: true # "centerline" in the Lanelet2 map will be used as a "waypoints" tag. lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path + dummy_metadata: # When the dynamic map loading is true and the map is a single file, this parameter will generate the metadata. + min_x: 0.0 + min_y: 0.0 + x_resolution: 100000.0 + y_resolution: 100000.0 diff --git a/map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp b/map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp new file mode 100644 index 0000000000000..18b3d0f6a71c5 --- /dev/null +++ b/map/map_loader/include/map_loader/lanelet2_differential_loader_module.hpp @@ -0,0 +1,86 @@ +// 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 MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ +#define MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ + +#include "autoware_lanelet2_extension/io/autoware_multi_osm_parser.hpp" +#include "map_loader/utils.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "autoware_map_msgs/srv/get_selected_lanelet2_map.hpp" +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +using GetDifferentialLanelet2Map = autoware_map_msgs::srv::GetSelectedLanelet2Map; +using autoware_map_msgs::msg::LaneletMapBin; + +class Lanelet2DifferentialLoaderModule +{ +public: + explicit Lanelet2DifferentialLoaderModule( + rclcpp::Node * node, const double & center_line_resolution); + + void setLaneletMapMetadata( + std::map & lanelet2_metadata_dict, double x_res, + double y_res); + + void setProjectionInfo(const tier4_map_msgs::msg::MapProjectorInfo & projector_info); + + lanelet::LaneletMapPtr differentialLanelet2Load(std::vector & lanelet2_paths); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + std::map lanelet2_file_metadata_dict_; + + rclcpp::Service::SharedPtr get_differential_lanelet2_maps_service_; + + component_interface_utils::Publisher::SharedPtr + pub_lanelet_map_meta_data_; + + std::optional projector_info_; + + double center_line_resolution_; + + bool onServiceGetDifferentialLanelet2Map( + GetDifferentialLanelet2Map::Request::SharedPtr req, + GetDifferentialLanelet2Map::Response::SharedPtr res); + + autoware_map_msgs::msg::LaneletMapMetaData getLaneletMapMetaDataMsg( + const double & x_res, const double & y_res) const; +}; + +#endif // MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp index e38d65201ee56..451682bee5863 100644 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ b/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 #include #include @@ -25,8 +28,10 @@ #include +#include #include #include +#include class Lanelet2MapLoaderNode : public rclcpp::Node { @@ -48,8 +53,20 @@ class Lanelet2MapLoaderNode : public rclcpp::Node void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); + std::unique_ptr differential_loader_module_; + component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; rclcpp::Publisher::SharedPtr pub_map_bin_; + + std::vector get_lanelet2_paths( + const std::vector & lanelet2_paths_or_directory) const; + std::map get_lanelet2_metadata( + const std::string & lanelet2_metadata_path, const std::vector & lanelet2_paths, + double & x_resolution, double & y_resolution) const; + std::map get_dummy_lanelet2_metadata( + const std::string & lanelet2_path, + const MapProjectorInfo::Message::ConstSharedPtr projection_info, double & x_resolution, + double & y_resolution); }; #endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/utils.hpp b/map/map_loader/include/map_loader/utils.hpp new file mode 100644 index 0000000000000..bd1618d941dd6 --- /dev/null +++ b/map/map_loader/include/map_loader/utils.hpp @@ -0,0 +1,35 @@ +// 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 MAP_LOADER__UTILS_HPP_ +#define MAP_LOADER__UTILS_HPP_ + +#include +#include +#include + +struct Lanelet2FileMetaData +{ + std::string id; + double min_x; + double min_y; +}; + +std::map loadLanelet2Metadata( + const std::string & lanelet2_metadata_path, double & x_resolution, double & y_resolution); +std::map replaceWithAbsolutePath( + const std::map & lanelet2_metadata_path, + const std::vector & lanelet2_paths); + +#endif // MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json index a55050e4ed570..c4ebaef68df62 100644 --- a/map/map_loader/schema/lanelet2_map_loader.schema.json +++ b/map/map_loader/schema/lanelet2_map_loader.schema.json @@ -6,6 +6,11 @@ "lanelet2_map_loader": { "type": "object", "properties": { + "enable_differential_map_loading": { + "type": "string", + "description": "Flag to enable loading of differential map", + "default": "true" + }, "allow_unsupported_version": { "type": "boolean", "description": "Flag to load unsupported format_version anyway. If true, just prints warning.", @@ -25,6 +30,32 @@ "type": "string", "description": "The lanelet2 map path pointing to the .osm file", "default": "" + }, + "dummy_metadata": { + "type": "object", + "properties": { + "min_x": { + "type": "number", + "description": "When the dynamic map loading is true and the map is a single file, this is the minimum x value of the map", + "default": "0.0" + }, + "min_y": { + "type": "number", + "description": "When the dynamic map loading is true and the map is a single file, this is the minimum y value of the map", + "default": "0.0" + }, + "x_resolution": { + "type": "number", + "description": "When the dynamic map loading is true and the map is a single file, this is the resolution of the x axis", + "default": "100000.0" + }, + "y_resolution": { + "type": "number", + "description": "When the dynamic map loading is true and the map is a single file, this is the resolution of the y axis", + "default": "100000.0" + } + }, + "required": ["min_x", "min_y", "x_resolution", "y_resolution"] } }, "required": ["center_line_resolution", "use_waypoints", "lanelet2_map_path"], diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp new file mode 100644 index 0000000000000..8879ac372a463 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp @@ -0,0 +1,155 @@ +// 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. + +#include "map_loader/lanelet2_differential_loader_module.hpp" + +#include "lanelet2_local_projector.hpp" +#include "map_loader/lanelet2_map_loader_node.hpp" + +Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule( + rclcpp::Node * node, const double & center_line_resolution) +: logger_(node->get_logger()), + clock_(node->get_clock()), + center_line_resolution_(center_line_resolution) +{ + const auto metadata_adaptor = component_interface_utils::NodeAdaptor(node); + metadata_adaptor.init_pub(pub_lanelet_map_meta_data_); + + get_differential_lanelet2_maps_service_ = node->create_service( + "service/get_differential_lanelet_map", + std::bind( + &Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map, this, + std::placeholders::_1, std::placeholders::_2)); +} + +bool Lanelet2DifferentialLoaderModule::onServiceGetDifferentialLanelet2Map( + GetDifferentialLanelet2Map::Request::SharedPtr req, + GetDifferentialLanelet2Map::Response::SharedPtr res) +{ + std::vector lanelet2_paths; + for (const auto & id : req->cell_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()) { + RCLCPP_ERROR(logger_, "Failed to load differential lanelet2 map"); + return false; + } + + const auto map = differentialLanelet2Load(lanelet2_paths); + const auto map_bin_msg = + Lanelet2MapLoaderNode::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now()); + + res->lanelet2_cells = map_bin_msg; + res->header.frame_id = "map"; + + return true; +} + +lanelet::LaneletMapPtr Lanelet2DifferentialLoaderModule::differentialLanelet2Load( + std::vector & lanelet2_paths) +{ + if (projector_info_.value().projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { + std::unique_ptr projector = + autoware::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); + } + } + + return map; + } 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); + } + } + + // 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); + } + + return map; + } +} + +void Lanelet2DifferentialLoaderModule::setLaneletMapMetadata( + std::map & lanelet2_metadata_dict, double x_res, double y_res) +{ + lanelet2_file_metadata_dict_ = lanelet2_metadata_dict; + + const auto msg = getLaneletMapMetaDataMsg(x_res, y_res); + pub_lanelet_map_meta_data_->publish(msg); +} + +void Lanelet2DifferentialLoaderModule::setProjectionInfo( + const tier4_map_msgs::msg::MapProjectorInfo & projector_info) +{ + projector_info_ = projector_info; +} + +autoware_map_msgs::msg::LaneletMapMetaData +Lanelet2DifferentialLoaderModule::getLaneletMapMetaDataMsg( + const double & x_res, const double & y_res) const +{ + autoware_map_msgs::msg::LaneletMapMetaData metadata; + for (const auto & file : lanelet2_file_metadata_dict_) { + autoware_map_msgs::msg::LaneletMapCellMetaData cell_msg; + cell_msg.cell_id = file.second.id; + cell_msg.min_x = file.second.min_x; + cell_msg.min_y = file.second.min_y; + cell_msg.max_x = file.second.min_x + x_res; + cell_msg.max_y = file.second.min_y + y_res; + + metadata.metadata_list.push_back(cell_msg); + } + metadata.header.frame_id = "map"; + metadata.header.stamp = clock_->now(); + + return metadata; +} diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 9704b2ec68150..4a53524bb764a 100644 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -49,9 +49,28 @@ #include #include +#include #include #include +namespace +{ +bool isOsmFile(const std::string & f) +{ + if (std::filesystem::is_directory(f)) { + return false; + } + + const std::string ext = std::filesystem::path(f).extension(); + + if (ext != ".osm" && ext != ".OSM") { + return false; + } + + return true; +} +} // namespace + using autoware_map_msgs::msg::LaneletMapBin; using tier4_map_msgs::msg::MapProjectorInfo; @@ -69,21 +88,78 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options declare_parameter("lanelet2_map_path"); declare_parameter("center_line_resolution"); declare_parameter("use_waypoints"); + declare_parameter("enable_differential_map_loading"); + + if (get_parameter("enable_differential_map_loading").as_bool()) { + differential_loader_module_ = std::make_unique( + this, get_parameter("center_line_resolution").as_double()); + } } void Lanelet2MapLoaderNode::on_map_projector_info( const MapProjectorInfo::Message::ConstSharedPtr msg) { const auto allow_unsupported_version = get_parameter("allow_unsupported_version").as_bool(); - const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); + auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); const auto use_waypoints = get_parameter("use_waypoints").as_bool(); + const auto enable_differential_map_loading = + get_parameter("enable_differential_map_loading").as_bool(); - // load map from file - const auto map = load_map(lanelet2_filename, *msg); - if (!map) { - RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published."); - return; + lanelet::LaneletMapPtr map; + + if (!enable_differential_map_loading) { + map = load_map(lanelet2_filename, *msg); + if (!map) { + RCLCPP_ERROR(get_logger(), "Failed to load lanelet2_map. Not published."); + return; + } + } else { + RCLCPP_INFO(get_logger(), "Differential lanelet2 map loading is enabled."); + + std::vector lanelet2_paths_or_directory = { + get_parameter("lanelet2_map_path").as_string()}; + std::string lanelet2_metadata_path = + declare_parameter("lanelet2_map_metadata_path"); + double x_resolution, y_resolution; + + std::map lanelet2_metadata_dict; + if (std::filesystem::exists(lanelet2_metadata_path)) { + lanelet2_metadata_dict = get_lanelet2_metadata( + lanelet2_metadata_path, get_lanelet2_paths(lanelet2_paths_or_directory), x_resolution, + y_resolution); + } else { + if (lanelet2_paths_or_directory.size() == 1) { + // Create a dummy metadata for a single osm file + lanelet2_metadata_dict = + get_dummy_lanelet2_metadata(lanelet2_filename, msg, x_resolution, y_resolution); + } else { + throw std::runtime_error("Lanelet2 metadata file not found: " + lanelet2_metadata_path); + } + } + + { + // set metadata and projection info to differential loader module + differential_loader_module_->setLaneletMapMetadata( + lanelet2_metadata_dict, x_resolution, y_resolution); + differential_loader_module_->setProjectionInfo(*msg); + } + + { + // load whole map for once + std::vector lanelet2_paths; + lanelet2_paths.reserve(lanelet2_metadata_dict.size()); + for (const auto & [lanelet2_path, _] : lanelet2_metadata_dict) { + lanelet2_paths.push_back(lanelet2_path); + } + map = differential_loader_module_->differentialLanelet2Load(lanelet2_paths); + } + lanelet2_filename = + lanelet2_metadata_dict.begin() + ->first; // TODO(StepTurtle): find better way: `parseVersions` function read the osm + // file to get map version info, so `lanelet2_filename` should + // be osm file path, it changes `lanelet2_filename` hard coded. + // Because of this, `lanelet2_filename` can't be const in line 103. } std::string format_version{"null"}, map_version{""}; @@ -200,5 +276,65 @@ LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( return map_bin_msg; } +std::vector Lanelet2MapLoaderNode::get_lanelet2_paths( + const std::vector & lanelet2_paths_or_directory) const +{ + std::vector lanelet2_paths; + for (const auto & path : lanelet2_paths_or_directory) { + if (!std::filesystem::exists(path)) { + RCLCPP_ERROR_STREAM(get_logger(), "No such file or directory: " << path); + continue; + } + + if (isOsmFile(path)) { + lanelet2_paths.push_back(path); + } + + if (std::filesystem::is_directory(path)) { + for (const auto & file : std::filesystem::directory_iterator(path)) { + const auto filename = file.path().string(); + if (isOsmFile(filename)) { + lanelet2_paths.push_back(filename); + } + } + } + } + return lanelet2_paths; +} + +std::map Lanelet2MapLoaderNode::get_lanelet2_metadata( + const std::string & lanelet2_metadata_path, const std::vector & lanelet2_paths, + double & x_resolution, double & y_resolution) const +{ + std::map lanelet2_metadata_dict; + lanelet2_metadata_dict = loadLanelet2Metadata(lanelet2_metadata_path, x_resolution, y_resolution); + lanelet2_metadata_dict = replaceWithAbsolutePath(lanelet2_metadata_dict, lanelet2_paths); + RCLCPP_INFO_STREAM(get_logger(), "Loaded Lanelet2 metadata: " << lanelet2_metadata_path); + + return lanelet2_metadata_dict; +} + +std::map Lanelet2MapLoaderNode::get_dummy_lanelet2_metadata( + const std::string & lanelet2_path, + const MapProjectorInfo::Message::ConstSharedPtr projection_info, double & x_resolution, + double & y_resolution) +{ + const auto map = load_map(lanelet2_path, *projection_info); + + declare_parameter("dummy_metadata.min_x"); + declare_parameter("dummy_metadata.min_y"); + declare_parameter("dummy_metadata.x_resolution"); + declare_parameter("dummy_metadata.y_resolution"); + + Lanelet2FileMetaData tile; + tile.id = "0"; + tile.min_x = get_parameter("dummy_metadata.min_x").as_double(); + tile.min_y = get_parameter("dummy_metadata.min_y").as_double(); + x_resolution = get_parameter("dummy_metadata.x_resolution").as_double(); + y_resolution = get_parameter("dummy_metadata.y_resolution").as_double(); + + return std::map{{lanelet2_path, tile}}; +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapLoaderNode) diff --git a/map/map_loader/src/lanelet2_map_loader/utils.cpp b/map/map_loader/src/lanelet2_map_loader/utils.cpp new file mode 100644 index 0000000000000..acc4498d53607 --- /dev/null +++ b/map/map_loader/src/lanelet2_map_loader/utils.cpp @@ -0,0 +1,65 @@ +// 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. + +#include "map_loader/utils.hpp" + +#include + +std::map loadLanelet2Metadata( + const std::string & lanelet2_metadata_path, double & x_resolution, double & y_resolution) +{ + YAML::Node config = YAML::LoadFile(lanelet2_metadata_path); + + std::map lanelet2_metadata; + + x_resolution = config["x_resolution"].as(); + y_resolution = config["y_resolution"].as(); + + for (const auto & node : config) { + if ( + node.first.as() == "x_resolution" || + node.first.as() == "y_resolution") { + continue; + } + + auto key = node.first.as(); + + Lanelet2FileMetaData metadata; + std::stringstream( + node.first.as().substr(0, node.first.as().find('.'))) >> + metadata.id; + metadata.min_x = node.second.as>()[0]; + metadata.min_y = node.second.as>()[1]; + + lanelet2_metadata[key] = metadata; + } + + return lanelet2_metadata; +} + +std::map replaceWithAbsolutePath( + const std::map & lanelet2_metadata_path, + const std::vector & lanelet2_paths) +{ + std::map absolute_path_map; + for (const auto & path : lanelet2_paths) { + std::string filename = path.substr(path.find_last_of("/\\") + 1); + auto it = lanelet2_metadata_path.find(filename); + if (it != lanelet2_metadata_path.end()) { + absolute_path_map[path] = it->second; + } + } + + return absolute_path_map; +}