diff --git a/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp index ced71d0eb47db..ecaeba06248f3 100644 --- a/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp +++ b/common/autoware_component_interface_specs_universe/include/autoware/component_interface_specs_universe/map.hpp @@ -17,6 +17,7 @@ #include +#include #include namespace autoware::component_interface_specs_universe::map @@ -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 autoware::component_interface_specs_universe::map #endif // AUTOWARE__COMPONENT_INTERFACE_SPECS_UNIVERSE__MAP_HPP_ diff --git a/common/autoware_component_interface_specs_universe/test/test_map.cpp b/common/autoware_component_interface_specs_universe/test/test_map.cpp index ed7932eb5f3a3..51d908118911e 100644 --- a/common/autoware_component_interface_specs_universe/test/test_map.cpp +++ b/common/autoware_component_interface_specs_universe/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 autoware::component_interface_specs_universe::map::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 2fbdbc694cacc..635715e642ea0 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -3,16 +3,25 @@ + + + + + + + + - + + @@ -34,7 +43,24 @@ + + + + + + + + + + + + @@ -42,6 +68,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..66c6d138b225f --- /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/autoware_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..d867e0f9717d7 --- /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 + +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_; + + autoware::component_interface_utils::Subscription< + autoware::component_interface_specs_universe::map::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 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..7094a230c368e --- /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_component_interface_specs_universe + autoware_component_interface_utils + autoware_geography_utils + autoware_map_msgs + + + 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..3972189983874 --- /dev/null +++ b/map/autoware_dynamic_lanelet_provider/src/dynamic_lanelet_provider.cpp @@ -0,0 +1,212 @@ +// 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" + +#include +#include +#include +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/autoware_map_loader/CMakeLists.txt b/map/autoware_map_loader/CMakeLists.txt index 9f8f8681488aa..3c0179bce4b46 100644 --- a/map/autoware_map_loader/CMakeLists.txt +++ b/map/autoware_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/lanelet2_map_loader_utils.cpp ) rclcpp_components_register_node(lanelet2_map_loader_node diff --git a/map/autoware_map_loader/README.md b/map/autoware_map_loader/README.md index 34c87289c4e5e..8a86b23fd3e3e 100644 --- a/map/autoware_map_loader/README.md +++ b/map/autoware_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 [autoware_map_msgs/msg/MapProjectorInfo.msg](https://github.com/autowarefoundation/autoware_msgs/blob/main/autoware_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 [autoware_map_msgs/msg/MapProjectorInfo.msg](https://github.com/autow ### 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/autoware_map_loader/schema/lanelet2_map_loader.schema.json") }} diff --git a/map/autoware_map_loader/config/lanelet2_map_loader.param.yaml b/map/autoware_map_loader/config/lanelet2_map_loader.param.yaml index 48d392a1b8e66..2fe25e8568dae 100755 --- a/map/autoware_map_loader/config/lanelet2_map_loader.param.yaml +++ b/map/autoware_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/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_utils.hpp b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_utils.hpp new file mode 100644 index 0000000000000..cf3a584c8e698 --- /dev/null +++ b/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_utils.hpp @@ -0,0 +1,60 @@ +// 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__MAP_LOADER__LANELET2_MAP_LOADER_UTILS_HPP_ +#define AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_UTILS_HPP_ + +#include + +#include +#include + +#include + +#include +#include +#include +namespace autoware::map_loader +{ +struct Lanelet2FileMetaData +{ + std::string id; + double min_x; + double min_y; +}; + +namespace utils +{ +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); + +void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap & merge_source); + +lanelet::LaneletMapPtr load_map( + const std::string & lanelet2_filename, + const autoware_map_msgs::msg::MapProjectorInfo & projector_info); + +autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, + const rclcpp::Time & now); + +} // namespace utils + +} // namespace autoware::map_loader + +#endif // AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_UTILS_HPP_ diff --git a/map/autoware_map_loader/schema/lanelet2_map_loader.schema.json b/map/autoware_map_loader/schema/lanelet2_map_loader.schema.json index a55050e4ed570..c4ebaef68df62 100644 --- a/map/autoware_map_loader/schema/lanelet2_map_loader.schema.json +++ b/map/autoware_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/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp new file mode 100644 index 0000000000000..5a5447bba4d0c --- /dev/null +++ b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.cpp @@ -0,0 +1,135 @@ +// 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 "lanelet2_differential_loader_module.hpp" + +#include "lanelet2_local_projector.hpp" +#include "lanelet2_map_loader_node.hpp" + +#include + +#include +#include +#include +#include + +namespace autoware::map_loader +{ + +Lanelet2DifferentialLoaderModule::Lanelet2DifferentialLoaderModule( + rclcpp::Node * node, const double & center_line_resolution, const bool & use_waypoints) +: logger_(node->get_logger()), + clock_(node->get_clock()), + center_line_resolution_(center_line_resolution), + use_waypoints_(use_waypoints) +{ + const auto metadata_adaptor = autoware::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) +{ + // get the list of lanelet2 map paths from the requested cell_ids + 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; + } + + // load the lanelet2 maps + lanelet::LaneletMapPtr map = std::make_shared(); + for (const auto & path : lanelet2_paths) { + auto map_tmp = utils::load_map(path, projector_info_.value()); + if (!map_tmp) { + RCLCPP_ERROR( + rclcpp::get_logger("map_loader"), "Failed to load lanelet2_map %s", path.c_str()); + return false; + } + utils::merge_lanelet2_maps(*map, *map_tmp); + } + + // overwrite centerline + if (use_waypoints_) { + lanelet::utils::overwriteLaneletsCenterlineWithWaypoints(map, center_line_resolution_, false); + } else { + lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution_, false); + } + + // create the map bin message + const auto map_bin_msg = utils::create_map_bin_msg(map, lanelet2_paths[0], rclcpp::Clock().now()); + + res->lanelet2_cells = map_bin_msg; + res->header.frame_id = "map"; + + return true; +} + +void Lanelet2DifferentialLoaderModule::setLaneletMapMetadata( + const std::map & lanelet2_metadata_dict, const double x_res, + const 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 autoware_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; +} + +} // namespace autoware::map_loader diff --git a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.hpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_differential_loader_module.hpp new file mode 100644 index 0000000000000..f201ebaac8318 --- /dev/null +++ b/map/autoware_map_loader/src/lanelet2_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 LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ +#define LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace autoware::map_loader +{ + +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, const bool & use_waypoints); + + void setLaneletMapMetadata( + const std::map & lanelet2_metadata_dict, const double x_res, + const double y_res); + + void setProjectionInfo(const autoware_map_msgs::msg::MapProjectorInfo & projector_info); + +private: + rclcpp::Logger logger_; + rclcpp::Clock::SharedPtr clock_; + + std::map lanelet2_file_metadata_dict_; + + rclcpp::Service::SharedPtr get_differential_lanelet2_maps_service_; + + autoware::component_interface_utils::Publisher< + autoware::component_interface_specs_universe::map::LaneletMapMetaData>::SharedPtr + pub_lanelet_map_meta_data_; + + std::optional projector_info_; + + double center_line_resolution_; + bool use_waypoints_; + 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; +}; + +} // namespace autoware::map_loader +#endif // LANELET2_MAP_LOADER__LANELET2_DIFFERENTIAL_LOADER_MODULE_HPP_ diff --git a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp index 47439508f46ae..75aca8c508f70 100644 --- a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp @@ -31,12 +31,13 @@ * */ -#include "autoware/map_loader/lanelet2_map_loader_node.hpp" +#include "lanelet2_map_loader_node.hpp" #include "lanelet2_local_projector.hpp" #include #include +#include #include #include #include @@ -49,12 +50,34 @@ #include #include +#include +#include #include #include #include +#include namespace autoware::map_loader { + +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 autoware_map_msgs::msg::MapProjectorInfo; @@ -69,34 +92,85 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); declare_parameter("allow_unsupported_version"); - declare_parameter("lanelet2_map_path"); + declare_parameter>("lanelet2_map_paths_or_directory"); 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(), + get_parameter("use_waypoints").as_bool()); + } } 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(); + const auto lanelet2_paths_or_directory = + get_parameter("lanelet2_map_paths_or_directory").as_string_array(); 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."); + // validate lanelet2_paths_or_directory + if (lanelet2_paths_or_directory.empty()) { + RCLCPP_ERROR(get_logger(), "No lanelet2 map files given to the node"); return; } + // get lanelet2 paths + const std::vector lanelet2_paths = get_lanelet2_paths(lanelet2_paths_or_directory); + if (lanelet2_paths.empty()) { + RCLCPP_ERROR( + get_logger(), "No lanelet2 map files found from %s", lanelet2_paths_or_directory[0].c_str()); + return; + } + + // setup differential map loader module + if (enable_differential_map_loading) { + RCLCPP_INFO(get_logger(), "Differential lanelet2 map loading is enabled."); + + // generate metadata + const auto 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, lanelet2_paths, 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 lanelet2 map + lanelet::LaneletMapPtr map = std::make_shared(); + 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."); + return; + } + utils::merge_lanelet2_maps(*map, *map_tmp); + } + + // we use first lanelet2 path to get format_version and map_version std::string format_version{"null"}, map_version{""}; lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); + lanelet2_paths[0], &format_version, &map_version); if (format_version == "null" || format_version.empty() || !isdigit(format_version[0])) { RCLCPP_WARN( get_logger(), "%s has no format_version(null) or non semver-style format_version(%s) information", - lanelet2_filename.c_str(), format_version.c_str()); + lanelet2_paths[0].c_str(), format_version.c_str()); if (!allow_unsupported_version) { throw std::invalid_argument( "allow_unsupported_version is false, so stop loading lanelet map"); @@ -108,7 +182,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( RCLCPP_WARN( get_logger(), "format_version(%ld) of the provided map(%s) is larger than the supported version(%ld)", - map_major_ver, lanelet2_filename.c_str(), + map_major_ver, lanelet2_paths[0].c_str(), static_cast(lanelet::autoware::version)); if (!allow_unsupported_version) { throw std::invalid_argument( @@ -126,7 +200,7 @@ void Lanelet2MapLoaderNode::on_map_projector_info( } // create map bin msg - const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); + const auto map_bin_msg = utils::create_map_bin_msg(map, lanelet2_paths[0], now()); // create publisher and publish pub_map_bin_ = @@ -135,73 +209,53 @@ void Lanelet2MapLoaderNode::on_map_projector_info( RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); } -lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, - const autoware_map_msgs::msg::MapProjectorInfo & projector_info) +/** + * @brief Get list of lanelet2 map file paths from input paths/directories + * @param lanelet2_paths_or_directory Vector of paths that can be either lanelet2 map files or + * directories containing them + * @return Vector of absolute paths to lanelet2 map files + */ +std::vector Lanelet2MapLoaderNode::get_lanelet2_paths( + const std::vector & lanelet2_paths_or_directory) const { - lanelet::ErrorMessages errors{}; - if (projector_info.projector_type != autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { - std::unique_ptr projector = - autoware::geography_utils::get_lanelet2_projector(projector_info); - lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); - if (errors.empty()) { - return map; + 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; } - } else { - const autoware::map_loader::LocalProjector projector; - lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors); - if (!errors.empty()) { - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); - } + // If the given path is already a lanelet2 map file, add it to the list + if (isOsmFile(path)) { + lanelet2_paths.push_back(path); } - // 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(); + // If the given path is a directory, add all the lanelet2 map files in the directory + 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); + } } } - - // 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; - } - - for (const auto & error : errors) { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); } - return nullptr; + return lanelet2_paths; } -LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +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::string format_version{}; - std::string map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); + std::map lanelet2_metadata_dict; + lanelet2_metadata_dict = + utils::loadLanelet2Metadata(lanelet2_metadata_path, x_resolution, y_resolution); + lanelet2_metadata_dict = utils::replaceWithAbsolutePath(lanelet2_metadata_dict, lanelet2_paths); + RCLCPP_INFO_STREAM(get_logger(), "Loaded Lanelet2 metadata: " << lanelet2_metadata_path); - LaneletMapBin map_bin_msg; - map_bin_msg.header.stamp = now; - map_bin_msg.header.frame_id = "map"; - map_bin_msg.version_map_format = format_version; - map_bin_msg.version_map = map_version; - lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); - - return map_bin_msg; + return lanelet2_metadata_dict; } + } // namespace autoware::map_loader #include diff --git a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.hpp similarity index 68% rename from map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp rename to map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.hpp index 2e0eb14c02eae..64a749caa3c8e 100644 --- a/map/autoware_map_loader/include/autoware/map_loader/lanelet2_map_loader_node.hpp +++ b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.hpp @@ -12,11 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ -#define AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#ifndef LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#define LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ + +#include "lanelet2_differential_loader_module.hpp" #include #include +#include #include #include @@ -25,8 +28,10 @@ #include +#include #include #include +#include namespace autoware::map_loader { @@ -38,13 +43,6 @@ class Lanelet2MapLoaderNode : public rclcpp::Node public: explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); - static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, - const autoware_map_msgs::msg::MapProjectorInfo & projector_info); - static autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, - const rclcpp::Time & now); - private: using MapProjectorInfo = autoware::component_interface_specs_universe::map::MapProjectorInfo; @@ -52,8 +50,16 @@ class Lanelet2MapLoaderNode : public rclcpp::Node autoware::component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; + std::unique_ptr differential_loader_module_; + 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; }; } // namespace autoware::map_loader -#endif // AUTOWARE__MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ +#endif // LANELET2_MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp new file mode 100644 index 0000000000000..a319d57390239 --- /dev/null +++ b/map/autoware_map_loader/src/lanelet2_map_loader/lanelet2_map_loader_utils.cpp @@ -0,0 +1,179 @@ +// 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 "lanelet2_local_projector.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware::map_loader::utils +{ + +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; +} + +lanelet::LaneletMapPtr load_map( + const std::string & lanelet2_filename, + const autoware_map_msgs::msg::MapProjectorInfo & projector_info) +{ + lanelet::ErrorMessages errors{}; + if (projector_info.projector_type != autoware_map_msgs::msg::MapProjectorInfo::LOCAL) { + std::unique_ptr projector = + autoware::geography_utils::get_lanelet2_projector(projector_info); + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); + if (errors.empty()) { + return map; + } + } else { + const autoware::map_loader::LocalProjector projector; + lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &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; + } + + for (const auto & error : errors) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_loader"), error); + } + return nullptr; +} + +void merge_lanelet2_maps(lanelet::LaneletMap & merge_target, lanelet::LaneletMap & merge_source) +{ + for (lanelet::Lanelet & lanelet : merge_source.laneletLayer) { + merge_target.add(lanelet); + } + for (const auto & area : merge_source.areaLayer) { + merge_target.add(area); + } + for (const auto & regulatory_element : merge_source.regulatoryElementLayer) { + merge_target.add(regulatory_element); + } + for (const auto & line_string : merge_source.lineStringLayer) { + merge_target.add(line_string); + } + for (const auto & polygon : merge_source.polygonLayer) { + merge_target.add(polygon); + } + for (const auto & point : merge_source.pointLayer) { + merge_target.add(point); + } +} + +autoware_map_msgs::msg::LaneletMapBin create_map_bin_msg( + const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) +{ + std::string format_version{}; + std::string map_version{}; + lanelet::io_handlers::AutowareOsmParser::parseVersions( + lanelet2_filename, &format_version, &map_version); + + autoware_map_msgs::msg::LaneletMapBin map_bin_msg; + map_bin_msg.header.stamp = now; + map_bin_msg.header.frame_id = "map"; + map_bin_msg.version_map_format = format_version; + map_bin_msg.version_map = map_version; + lanelet::utils::conversion::toBinMsg(map, &map_bin_msg); + + return map_bin_msg; +} + +} // namespace autoware::map_loader::utils diff --git a/map/autoware_map_loader/test/lanelet2_map_loader_launch.test.py b/map/autoware_map_loader/test/lanelet2_map_loader_launch.test.py index 14f05b1938b61..1d80f34dfdbe5 100644 --- a/map/autoware_map_loader/test/lanelet2_map_loader_launch.test.py +++ b/map/autoware_map_loader/test/lanelet2_map_loader_launch.test.py @@ -36,7 +36,8 @@ def generate_test_description(): executable="autoware_lanelet2_map_loader", parameters=[ { - "lanelet2_map_path": lanelet2_map_path, + "enable_differential_map_loading": False, + "lanelet2_map_paths_or_directory": [lanelet2_map_path], "center_line_resolution": 5.0, "use_waypoints": True, "allow_unsupported_version": True,