diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt deleted file mode 100644 index 115f7e5b17464..0000000000000 --- a/map/map_loader/CMakeLists.txt +++ /dev/null @@ -1,86 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(map_loader) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -find_package(PCL REQUIRED COMPONENTS common io filters) - -ament_auto_add_library(pointcloud_map_loader_node SHARED - src/pointcloud_map_loader/pointcloud_map_loader_node.cpp - src/pointcloud_map_loader/pointcloud_map_loader_module.cpp - src/pointcloud_map_loader/partial_map_loader_module.cpp - src/pointcloud_map_loader/differential_map_loader_module.cpp - src/pointcloud_map_loader/selected_map_loader_module.cpp - src/pointcloud_map_loader/utils.cpp -) -target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) -target_link_libraries(pointcloud_map_loader_node yaml-cpp) - -target_include_directories(pointcloud_map_loader_node - SYSTEM PUBLIC - ${PCL_INCLUDE_DIRS} -) - -rclcpp_components_register_node(pointcloud_map_loader_node - PLUGIN "PointCloudMapLoaderNode" - EXECUTABLE pointcloud_map_loader -) - -ament_auto_add_library(lanelet2_map_loader_node SHARED - src/lanelet2_map_loader/lanelet2_map_loader_node.cpp -) - -rclcpp_components_register_node(lanelet2_map_loader_node - PLUGIN "Lanelet2MapLoaderNode" - EXECUTABLE lanelet2_map_loader -) - -ament_auto_add_library(lanelet2_map_visualization_node SHARED - src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp -) - -rclcpp_components_register_node(lanelet2_map_visualization_node - PLUGIN "Lanelet2MapVisualizationNode" - EXECUTABLE lanelet2_map_visualization -) - -if(BUILD_TESTING) - add_launch_test( - test/lanelet2_map_loader_launch.test.py - TIMEOUT "30" - ) - install(DIRECTORY - test/data/ - DESTINATION share/${PROJECT_NAME}/test/data/ - ) - - function(add_testcase filepath) - get_filename_component(filename ${filepath} NAME) - string(REGEX REPLACE ".cpp" "" test_name ${filename}) - ament_add_gmock(${test_name} ${filepath}) - target_include_directories(${test_name} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/include) - target_link_libraries(${test_name} ${${PROJECT_NAME}_LIBRARIES}) - target_link_libraries(${test_name} yaml-cpp) - ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) - endfunction() - - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - add_testcase(test/test_cylinder_box_overlap.cpp) - add_testcase(test/test_replace_with_absolute_path.cpp) - add_testcase(test/test_load_pcd_metadata.cpp) - add_testcase(test/test_pointcloud_map_loader_module.cpp) - add_testcase(test/test_partial_map_loader_module.cpp) - add_testcase(test/test_differential_map_loader_module.cpp) -endif() - -install(PROGRAMS - script/map_hash_generator - DESTINATION lib/${PROJECT_NAME} -) - -ament_auto_package(INSTALL_TO_SHARE - launch - config -) diff --git a/map/map_loader/README.md b/map/map_loader/README.md deleted file mode 100644 index 21bd39303250f..0000000000000 --- a/map/map_loader/README.md +++ /dev/null @@ -1,171 +0,0 @@ -# map_loader package - -This package provides the features of loading various maps. - -## pointcloud_map_loader - -### Feature - -`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. -Currently, it supports the following two types: - -- Publish raw pointcloud map -- Publish downsampled pointcloud map -- Send partial pointcloud map loading via ROS 2 service -- Send differential pointcloud map loading via ROS 2 service - -NOTE: **We strongly recommend to use divided maps when using large pointcloud map to enable the latter two features (partial and differential load). Please go through the prerequisites section for more details, and follow the instruction for dividing the map and preparing the metadata.** - -### Prerequisites - -#### Prerequisites on pointcloud map file(s) - -You may provide either a single .pcd file or multiple .pcd files. If you are using multiple PCD data, it MUST obey the following rules: - -1. **The pointcloud map should be projected on the same coordinate defined in `map_projection_loader`**, in order to be consistent with the lanelet2 map and other packages that converts between local and geodetic coordinates. For more information, please refer to [the readme of `map_projection_loader`](https://github.com/autowarefoundation/autoware.universe/tree/main/map/map_projection_loader/README.md). -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. **The division size along each axis should be equal.** -4. **The division size should be about 20m x 20m.** Particularly, care should be taken as using too large division size (for example, more than 100m) may have adverse effects on dynamic map loading features in [ndt_scan_matcher](https://github.com/autowarefoundation/autoware.universe/tree/main/localization/ndt_scan_matcher) and [compare_map_segmentation](https://github.com/autowarefoundation/autoware.universe/tree/main/perception/compare_map_segmentation). -5. **All the split maps should not overlap with each other.** -6. **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.pcd: [1200, 2500] # -> 1200 < x < 1220, 2500 < y < 2520 -B.pcd: [1220, 2500] # -> 1220 < x < 1240, 2500 < y < 2520 -C.pcd: [1200, 2520] # -> 1200 < x < 1220, 2520 < y < 2540 -D.pcd: [1240, 2520] # -> 1240 < x < 1260, 2520 < y < 2540 -``` - -where, - -- `x_resolution` and `y_resolution` -- `A.pcd`, `B.pcd`, etc, are the names of PCD files. -- List such as `[1200, 2500]` are the values indicate that for this PCD 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 [pointcloud_divider](https://github.com/MapIV/pointcloud_divider) from MAP IV for dividing pointcloud map as well as generating the compatible metadata.yaml. - -#### Directory structure of these files - -If you only have one pointcloud map, Autoware will assume the following directory structure by default. - -```bash -sample-map-rosbag -├── lanelet2_map.osm -├── pointcloud_map.pcd -``` - -If you have multiple rosbags, an example directory structure would be as follows. Note that you need to have a metadata when you have multiple pointcloud map files. - -```bash -sample-map-rosbag -├── lanelet2_map.osm -├── pointcloud_map.pcd -│ ├── A.pcd -│ ├── B.pcd -│ ├── C.pcd -│ └── ... -├── map_projector_info.yaml -└── pointcloud_map_metadata.yaml -``` - -### Specific features - -#### Publish raw pointcloud map (ROS 2 topic) - -The node publishes the raw pointcloud map loaded from the `.pcd` file(s). - -#### Publish downsampled pointcloud map (ROS 2 topic) - -The node publishes the downsampled pointcloud map loaded from the `.pcd` file(s). You can specify the downsample resolution by changing the `leaf_size` parameter. - -#### Publish metadata of pointcloud map (ROS 2 topic) - -The node publishes the pointcloud metadata attached with an ID. Metadata is loaded from the `.yaml` file. Please see [the description of `PointCloudMapMetaData.msg`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#pointcloudmapmetadatamsg) for details. - -#### Send partial pointcloud map (ROS 2 service) - -Here, we assume that the pointcloud maps are divided into grids. - -Given a query from a client node, the node sends a set of pointcloud maps that overlaps with the queried area. -Please see [the description of `GetPartialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getpartialpointcloudmapsrv) for details. - -#### Send differential pointcloud map (ROS 2 service) - -Here, we assume that the pointcloud maps are divided into grids. - -Given a query and set of map IDs, the node sends a set of pointcloud maps that overlap with the queried area and are not included in the set of map IDs. -Please see [the description of `GetDifferentialPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getdifferentialpointcloudmapsrv) for details. - -#### Send selected pointcloud map (ROS 2 service) - -Here, we assume that the pointcloud maps are divided into grids. - -Given IDs query from a client node, the node sends a set of pointcloud maps (each of which attached with unique ID) specified by query. -Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.com/autowarefoundation/autoware_msgs/tree/main/autoware_map_msgs#getselectedpointcloudmapsrv) for details. - -### Parameters - -{{ json_to_markdown("map/map_loader/schema/pointcloud_map_loader.schema.json") }} - -### Interfaces - -- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map -- `output/pointcloud_map_metadata` (autoware_map_msgs/msg/PointCloudMapMetaData) : Metadata of pointcloud map -- `output/debug/downsampled_pointcloud_map` (sensor_msgs/msg/PointCloud2) : Downsampled pointcloud map -- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map -- `service/get_differential_pcd_map` (autoware_map_msgs/srv/GetDifferentialPointCloudMap) : Differential pointcloud map -- `service/get_selected_pcd_map` (autoware_map_msgs/srv/GetSelectedPointCloudMap) : Selected pointcloud map -- pointcloud map file(s) (.pcd) -- metadata of pointcloud map(s) (.yaml) - ---- - -## lanelet2_map_loader - -### Feature - -lanelet2_map_loader loads Lanelet2 file 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. - -### How to run - -`ros2 run map_loader lanelet2_map_loader --ros-args -p lanelet2_map_path:=path/to/map.osm` - -### Subscribed Topics - -- ~input/map_projector_info (tier4_map_msgs/MapProjectorInfo) : Projection type for Autoware - -### Published Topics - -- ~output/lanelet2_map (autoware_map_msgs/LaneletMapBin) : Binary data of loaded Lanelet2 Map - -### Parameters - -{{ json_to_markdown("map/map_loader/schema/lanelet2_map_loader.schema.json") }} - ---- - -## lanelet2_map_visualization - -### Feature - -lanelet2_map_visualization visualizes autoware_map_msgs/LaneletMapBin messages into visualization_msgs/MarkerArray. - -### How to Run - -`ros2 run map_loader lanelet2_map_visualization` - -### Subscribed Topics - -- ~input/lanelet2_map (autoware_map_msgs/LaneletMapBin) : binary data of Lanelet2 Map - -### Published Topics - -- ~output/lanelet2_map_marker (visualization_msgs/MarkerArray) : visualization messages for RViz diff --git a/map/map_loader/config/lanelet2_map_loader.param.yaml b/map/map_loader/config/lanelet2_map_loader.param.yaml deleted file mode 100755 index b830e038f1de2..0000000000000 --- a/map/map_loader/config/lanelet2_map_loader.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - center_line_resolution: 5.0 # [m] - lanelet2_map_path: $(var lanelet2_map_path) # The lanelet2 map path diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml deleted file mode 100644 index 180a3e6f9a2e5..0000000000000 --- a/map/map_loader/config/pointcloud_map_loader.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - enable_whole_load: true - enable_downsampled_whole_load: false - enable_partial_load: true - enable_selected_load: false - - # only used when downsample_whole_load enabled - leaf_size: 3.0 # downsample leaf size [m] - pcd_paths_or_directory: [$(var pcd_paths_or_directory)] # Path to the pointcloud map file or directory - pcd_metadata_path: $(var pcd_metadata_path) # Path to pointcloud metadata file 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 deleted file mode 100644 index 4e85ddec056c1..0000000000000 --- a/map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// 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_MAP_LOADER_NODE_HPP_ -#define MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ - -#include -#include -#include - -#include -#include - -#include - -#include -#include - -using autoware_map_msgs::msg::LaneletMapBin; -using tier4_map_msgs::msg::MapProjectorInfo; - -class Lanelet2MapLoaderNode : public rclcpp::Node -{ -public: - explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options); - - static lanelet::LaneletMapPtr load_map( - const std::string & lanelet2_filename, - const tier4_map_msgs::msg::MapProjectorInfo & projector_info); - static LaneletMapBin create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, - const rclcpp::Time & now); - -private: - using MapProjectorInfo = map_interface::MapProjectorInfo; - - void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg); - - component_interface_utils::Subscription::SharedPtr sub_map_projector_info_; - rclcpp::Publisher::SharedPtr pub_map_bin_; -}; - -#endif // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp b/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp deleted file mode 100644 index cb640e4dc83d5..0000000000000 --- a/map/map_loader/include/map_loader/lanelet2_map_visualization_node.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// 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_MAP_VISUALIZATION_NODE_HPP_ -#define MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ - -#include - -#include -#include - -#include -#include - -class Lanelet2MapVisualizationNode : public rclcpp::Node -{ -public: - explicit Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options); - -private: - rclcpp::Subscription::SharedPtr sub_map_bin_; - rclcpp::Publisher::SharedPtr pub_marker_; - - bool viz_lanelets_centerline_; - - void onMapBin(const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg); -}; - -#endif // MAP_LOADER__LANELET2_MAP_VISUALIZATION_NODE_HPP_ diff --git a/map/map_loader/launch/lanelet2_map_loader.launch.xml b/map/map_loader/launch/lanelet2_map_loader.launch.xml deleted file mode 100644 index 5baee911d6572..0000000000000 --- a/map/map_loader/launch/lanelet2_map_loader.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml deleted file mode 100644 index 3633a8fa39a6b..0000000000000 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml deleted file mode 100644 index a6f4ac7f81275..0000000000000 --- a/map/map_loader/package.xml +++ /dev/null @@ -1,46 +0,0 @@ - - - - map_loader - 0.1.0 - The map_loader package - Yamato Ando - Ryu Yamamoto - Koji Minoda - Masahiro Sakamoto - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Shintaro Sakoda - - Apache License 2.0 - Ryohsuke Mitsudome - Koji Minoda - - ament_cmake_auto - autoware_cmake - - autoware_map_msgs - component_interface_specs - component_interface_utils - fmt - geography_utils - geometry_msgs - lanelet2_extension - libpcl-all-dev - pcl_conversions - rclcpp - rclcpp_components - tier4_map_msgs - visualization_msgs - yaml-cpp - - ament_cmake_gmock - ament_lint_auto - autoware_lint_common - ros_testing - - - ament_cmake - - diff --git a/map/map_loader/schema/lanelet2_map_loader.schema.json b/map/map_loader/schema/lanelet2_map_loader.schema.json deleted file mode 100644 index fa2b4d363ff92..0000000000000 --- a/map/map_loader/schema/lanelet2_map_loader.schema.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for lanelet2 map loader Node", - "type": "object", - "definitions": { - "lanelet2_map_loader": { - "type": "object", - "properties": { - "center_line_resolution": { - "type": "number", - "description": "Resolution of the Lanelet center line [m]", - "default": "5.0" - }, - "lanelet2_map_path": { - "type": "string", - "description": "The lanelet2 map path pointing to the .osm file", - "default": "" - } - }, - "required": ["center_line_resolution", "lanelet2_map_path"], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/lanelet2_map_loader" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/map/map_loader/schema/pointcloud_map_loader.schema.json b/map/map_loader/schema/pointcloud_map_loader.schema.json deleted file mode 100644 index 03cf10b9c5423..0000000000000 --- a/map/map_loader/schema/pointcloud_map_loader.schema.json +++ /dev/null @@ -1,71 +0,0 @@ -{ - "$schema": "http://json-schema.org/draft-07/schema#", - "title": "Parameters for point cloud map loader Node", - "type": "object", - "definitions": { - "pointcloud_map_loader": { - "type": "object", - "properties": { - "enable_whole_load": { - "type": "boolean", - "description": "Enable raw pointcloud map publishing", - "default": true - }, - "enable_downsampled_whole_load": { - "type": "boolean", - "description": "Enable downsampled pointcloud map publishing", - "default": false - }, - "enable_partial_load": { - "type": "boolean", - "description": "Enable partial pointcloud map server", - "default": true - }, - "enable_selected_load": { - "type": "boolean", - "description": "Enable selected pointcloud map server", - "default": false - }, - "leaf_size": { - "type": "number", - "description": "Downsampling leaf size (only used when enable_downsampled_whole_load is set true)", - "default": 3.0 - }, - "pcd_paths_or_directory": { - "type": "array", - "description": "Path(s) to pointcloud map file or directory", - "default": [] - }, - "pcd_metadata_path": { - "type": "string", - "description": "Path to pointcloud metadata file", - "default": "" - } - }, - "required": [ - "enable_whole_load", - "enable_downsampled_whole_load", - "enable_partial_load", - "enable_selected_load", - "leaf_size", - "pcd_paths_or_directory", - "pcd_metadata_path" - ], - "additionalProperties": false - } - }, - "properties": { - "/**": { - "type": "object", - "properties": { - "ros__parameters": { - "$ref": "#/definitions/pointcloud_map_loader" - } - }, - "required": ["ros__parameters"], - "additionalProperties": false - } - }, - "required": ["/**"], - "additionalProperties": false -} diff --git a/map/map_loader/script/map_hash_generator b/map/map_loader/script/map_hash_generator deleted file mode 100755 index 5fb57553604e3..0000000000000 --- a/map/map_loader/script/map_hash_generator +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2021 Tier IV, Inc. -# -# 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. - -import hashlib -import pathlib - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from tier4_external_api_msgs.msg import MapHash -from tier4_external_api_msgs.msg import ResponseStatus -from tier4_external_api_msgs.srv import GetTextFile - - -class MapHashGenerator(Node): - def __init__(self): - super().__init__("map_hash_generator") - self.lanelet_path = self.declare_parameter("lanelet2_map_path", "").value - self.lanelet_text = self.load_lanelet_text(self.lanelet_path) - self.lanelet_hash = self.generate_lanelet_file_hash(self.lanelet_text) - - self.pcd_map_path = self.declare_parameter("pointcloud_map_path", "").value - self.pcd_map_hash = self.generate_pcd_file_hash(self.pcd_map_path) - - qos_profile = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - msg = MapHash() - msg.lanelet = self.lanelet_hash - msg.pcd = self.pcd_map_hash - self.pub = self.create_publisher(MapHash, "/api/autoware/get/map/info/hash", qos_profile) - self.pub.publish(msg) - - self.srv = self.create_service( - GetTextFile, "/api/autoware/get/map/lanelet/xml", self.on_get_lanelet_xml - ) # noqa: E501 - - def on_get_lanelet_xml(self, request, response): - response.status.code = ResponseStatus.SUCCESS - response.file.text = self.lanelet_text - return response - - @staticmethod - def load_lanelet_text(path): - path = pathlib.Path(path) - return path.read_text() if path.is_file() else "" - - @staticmethod - def generate_lanelet_file_hash(data): - return hashlib.sha256(data.encode()).hexdigest() if data else "" - - def update_hash(self, m, path): - try: - with open(path, "rb") as pcd_file: - m.update(pcd_file.read()) - except FileNotFoundError as e: - self.get_logger().error(e) - return False - return True - - def generate_pcd_file_hash(self, path): - path = pathlib.Path(path) - if path.is_file(): - if not path.suffix == ".pcd": - self.get_logger().error(f"[{path}] is not pcd file") - return "" - m = hashlib.sha256() - if not self.update_hash(m, path): - return "" - return m.hexdigest() - - if path.is_dir(): - m = hashlib.sha256() - for pcd_file_path in sorted(path.iterdir()): - if not pcd_file_path.suffix == ".pcd": - continue - if not self.update_hash(m, pcd_file_path): - return "" - if m.hexdigest() == hashlib.sha256().hexdigest(): - self.get_logger().error(f"there are no pcd files in [{path}]") - return "" - return m.hexdigest() - - self.get_logger().error(f"[{path}] is neither file nor directory") - return "" - - -def main(args=None): - rclpy.init(args=args) - node = MapHashGenerator() - rclpy.spin(node) - rclpy.shutdown() - - -if __name__ == "__main__": - try: - main() - except KeyboardInterrupt: - pass diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp deleted file mode 100644 index 225445d17bfa1..0000000000000 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_local_projector.hpp +++ /dev/null @@ -1,41 +0,0 @@ -// Copyright 2023 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_LOCAL_PROJECTOR_HPP_ -#define LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ - -#include - -namespace lanelet::projection -{ - -class LocalProjector : public Projector -{ -public: - LocalProjector() : Projector(Origin(GPSPoint{})) {} - - BasicPoint3d forward(const GPSPoint & gps) const override // NOLINT - { - return BasicPoint3d{0.0, 0.0, gps.ele}; - } - - GPSPoint reverse(const BasicPoint3d & point) const override - { - return GPSPoint{0.0, 0.0, point.z()}; - } -}; - -} // namespace lanelet::projection - -#endif // LANELET2_MAP_LOADER__LANELET2_LOCAL_PROJECTOR_HPP_ 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 deleted file mode 100644 index b097e1809a385..0000000000000 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp +++ /dev/null @@ -1,162 +0,0 @@ -// Copyright 2021 TierIV -// -// 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. - -/* - * Copyright 2019 Autoware Foundation. All rights reserved. - * - * 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. - * - * Authors: Simon Thompson, Ryohsuke Mitsudome - * - */ - -#include "map_loader/lanelet2_map_loader_node.hpp" - -#include "lanelet2_local_projector.hpp" - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options) -: Node("lanelet2_map_loader", options) -{ - const auto adaptor = component_interface_utils::NodeAdaptor(this); - - // subscription - adaptor.init_sub( - sub_map_projector_info_, - [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); }); - - declare_parameter("lanelet2_map_path"); - declare_parameter("center_line_resolution"); -} - -void Lanelet2MapLoaderNode::on_map_projector_info( - const MapProjectorInfo::Message::ConstSharedPtr msg) -{ - const auto lanelet2_filename = get_parameter("lanelet2_map_path").as_string(); - const auto center_line_resolution = get_parameter("center_line_resolution").as_double(); - - // 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; - } - - // overwrite centerline - lanelet::utils::overwriteLaneletsCenterline(map, center_line_resolution, false); - - // create map bin msg - const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now()); - - // create publisher and publish - pub_map_bin_ = - create_publisher("output/lanelet2_map", rclcpp::QoS{1}.transient_local()); - pub_map_bin_->publish(map_bin_msg); - RCLCPP_INFO(get_logger(), "Succeeded to load lanelet2_map. Map is published."); -} - -lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map( - const std::string & lanelet2_filename, - const tier4_map_msgs::msg::MapProjectorInfo & projector_info) -{ - lanelet::ErrorMessages errors{}; - if (projector_info.projector_type != tier4_map_msgs::msg::MapProjectorInfo::LOCAL) { - std::unique_ptr projector = - geography_utils::get_lanelet2_projector(projector_info); - const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, *projector, &errors); - if (errors.empty()) { - return map; - } - } else { - const lanelet::projection::LocalProjector projector; - const 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; -} - -LaneletMapBin Lanelet2MapLoaderNode::create_map_bin_msg( - const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now) -{ - std::string format_version{}, map_version{}; - lanelet::io_handlers::AutowareOsmParser::parseVersions( - lanelet2_filename, &format_version, &map_version); - - 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; -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapLoaderNode) diff --git a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp b/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp deleted file mode 100644 index c81236bec86c0..0000000000000 --- a/map/map_loader/src/lanelet2_map_loader/lanelet2_map_visualization_node.cpp +++ /dev/null @@ -1,277 +0,0 @@ -// Copyright 2021 Tier IV, Inc. -// -// 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. - -/* - * Copyright 2019 Autoware Foundation. All rights reserved. - * - * 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. - * - * Authors: Simon Thompson, Ryohsuke Mitsudome - * - */ - -#include "map_loader/lanelet2_map_visualization_node.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -namespace -{ -void insertMarkerArray( - visualization_msgs::msg::MarkerArray * a1, const visualization_msgs::msg::MarkerArray & a2) -{ - a1->markers.insert(a1->markers.end(), a2.markers.begin(), a2.markers.end()); -} - -void setColor(std_msgs::msg::ColorRGBA * cl, double r, double g, double b, double a) -{ - cl->r = r; - cl->g = g; - cl->b = b; - cl->a = a; -} -} // namespace - -Lanelet2MapVisualizationNode::Lanelet2MapVisualizationNode(const rclcpp::NodeOptions & options) -: Node("lanelet2_map_visualization", options) -{ - using std::placeholders::_1; - - viz_lanelets_centerline_ = true; - - sub_map_bin_ = this->create_subscription( - "input/lanelet2_map", rclcpp::QoS{1}.transient_local(), - std::bind(&Lanelet2MapVisualizationNode::onMapBin, this, _1)); - - pub_marker_ = this->create_publisher( - "output/lanelet2_map_marker", rclcpp::QoS{1}.transient_local()); -} - -void Lanelet2MapVisualizationNode::onMapBin( - const autoware_map_msgs::msg::LaneletMapBin::ConstSharedPtr msg) -{ - lanelet::LaneletMapPtr viz_lanelet_map(new lanelet::LaneletMap); - - lanelet::utils::conversion::fromBinMsg(*msg, viz_lanelet_map); - RCLCPP_INFO(this->get_logger(), "Map is loaded\n"); - - // get lanelets etc to visualize - lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(viz_lanelet_map); - lanelet::ConstLanelets road_lanelets = lanelet::utils::query::roadLanelets(all_lanelets); - lanelet::ConstLanelets shoulder_lanelets = lanelet::utils::query::shoulderLanelets(all_lanelets); - lanelet::ConstLanelets crosswalk_lanelets = - lanelet::utils::query::crosswalkLanelets(all_lanelets); - lanelet::ConstLineStrings3d partitions = lanelet::utils::query::getAllPartitions(viz_lanelet_map); - lanelet::ConstLineStrings3d pedestrian_polygon_markings = - lanelet::utils::query::getAllPedestrianPolygonMarkings(viz_lanelet_map); - lanelet::ConstLineStrings3d pedestrian_line_markings = - lanelet::utils::query::getAllPedestrianLineMarkings(viz_lanelet_map); - lanelet::ConstLanelets walkway_lanelets = lanelet::utils::query::walkwayLanelets(all_lanelets); - std::vector stop_lines = - lanelet::utils::query::stopLinesLanelets(road_lanelets); - std::vector tl_reg_elems = - lanelet::utils::query::trafficLights(all_lanelets); - std::vector aw_tl_reg_elems = - lanelet::utils::query::autowareTrafficLights(all_lanelets); - std::vector da_reg_elems = - lanelet::utils::query::detectionAreas(all_lanelets); - std::vector no_reg_elems = - lanelet::utils::query::noStoppingAreas(all_lanelets); - std::vector sb_reg_elems = - lanelet::utils::query::speedBumps(all_lanelets); - std::vector cw_reg_elems = - lanelet::utils::query::crosswalks(all_lanelets); - lanelet::ConstLineStrings3d parking_spaces = - lanelet::utils::query::getAllParkingSpaces(viz_lanelet_map); - lanelet::ConstPolygons3d parking_lots = lanelet::utils::query::getAllParkingLots(viz_lanelet_map); - lanelet::ConstPolygons3d obstacle_polygons = - lanelet::utils::query::getAllObstaclePolygons(viz_lanelet_map); - lanelet::ConstPolygons3d no_obstacle_segmentation_area = - lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "no_obstacle_segmentation_area"); - lanelet::ConstPolygons3d no_obstacle_segmentation_area_for_run_out = - lanelet::utils::query::getAllPolygonsByType( - viz_lanelet_map, "no_obstacle_segmentation_area_for_run_out"); - lanelet::ConstPolygons3d hatched_road_markings_area = - lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "hatched_road_markings"); - lanelet::ConstPolygons3d intersection_areas = - lanelet::utils::query::getAllPolygonsByType(viz_lanelet_map, "intersection_area"); - std::vector no_parking_reg_elems = - lanelet::utils::query::noParkingAreas(all_lanelets); - lanelet::ConstLineStrings3d curbstones = lanelet::utils::query::curbstones(viz_lanelet_map); - - std_msgs::msg::ColorRGBA cl_road, cl_shoulder, cl_cross, cl_partitions, cl_pedestrian_markings, - cl_ll_borders, cl_shoulder_borders, cl_stoplines, cl_trafficlights, cl_detection_areas, - cl_speed_bumps, cl_crosswalks, cl_parking_lots, cl_parking_spaces, cl_lanelet_id, - cl_obstacle_polygons, cl_no_stopping_areas, cl_no_obstacle_segmentation_area, - cl_no_obstacle_segmentation_area_for_run_out, cl_hatched_road_markings_area, - cl_hatched_road_markings_line, cl_no_parking_areas, cl_curbstones, cl_intersection_area; - setColor(&cl_road, 0.27, 0.27, 0.27, 0.999); - setColor(&cl_shoulder, 0.15, 0.15, 0.15, 0.999); - setColor(&cl_cross, 0.27, 0.3, 0.27, 0.5); - setColor(&cl_partitions, 0.25, 0.25, 0.25, 0.999); - setColor(&cl_pedestrian_markings, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_ll_borders, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_shoulder_borders, 0.2, 0.2, 0.2, 0.999); - setColor(&cl_stoplines, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_trafficlights, 0.5, 0.5, 0.5, 0.8); - setColor(&cl_detection_areas, 0.27, 0.27, 0.37, 0.5); - setColor(&cl_no_stopping_areas, 0.37, 0.37, 0.37, 0.5); - setColor(&cl_speed_bumps, 0.56, 0.40, 0.27, 0.5); - setColor(&cl_crosswalks, 0.80, 0.80, 0.0, 0.5); - setColor(&cl_obstacle_polygons, 0.4, 0.27, 0.27, 0.5); - setColor(&cl_parking_lots, 0.5, 0.5, 0.0, 0.3); - setColor(&cl_parking_spaces, 1.0, 0.647, 0.0, 0.6); - setColor(&cl_lanelet_id, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_obstacle_segmentation_area, 0.37, 0.37, 0.27, 0.5); - setColor(&cl_no_obstacle_segmentation_area_for_run_out, 0.37, 0.7, 0.27, 0.5); - setColor(&cl_hatched_road_markings_area, 0.3, 0.3, 0.3, 0.5); - setColor(&cl_hatched_road_markings_line, 0.5, 0.5, 0.5, 0.999); - setColor(&cl_no_parking_areas, 0.42, 0.42, 0.42, 0.5); - setColor(&cl_curbstones, 0.1, 0.1, 0.2, 0.999); - setColor(&cl_intersection_area, 0.16, 1.0, 0.69, 0.5); - - visualization_msgs::msg::MarkerArray map_marker_array; - - insertMarkerArray( - &map_marker_array, - lanelet::visualization::lineStringsAsMarkerArray(stop_lines, "stop_lines", cl_stoplines, 0.5)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::lineStringsAsMarkerArray(partitions, "partitions", cl_partitions, 0.1)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::laneletDirectionAsMarkerArray(shoulder_lanelets, "shoulder_")); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletDirectionAsMarkerArray(road_lanelets)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( - "crosswalk_lanelets", crosswalk_lanelets, cl_cross)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::pedestrianPolygonMarkingsAsMarkerArray( - pedestrian_polygon_markings, cl_pedestrian_markings)); - - insertMarkerArray( - &map_marker_array, lanelet::visualization::pedestrianLineMarkingsAsMarkerArray( - pedestrian_line_markings, cl_pedestrian_markings)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( - "walkway_lanelets", walkway_lanelets, cl_cross)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::obstaclePolygonsAsMarkerArray(obstacle_polygons, cl_obstacle_polygons)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::detectionAreasAsMarkerArray(da_reg_elems, cl_detection_areas)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::noStoppingAreasAsMarkerArray(no_reg_elems, cl_no_stopping_areas)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::speedBumpsAsMarkerArray(sb_reg_elems, cl_speed_bumps)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::crosswalkAreasAsMarkerArray(cw_reg_elems, cl_crosswalks)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::parkingLotsAsMarkerArray(parking_lots, cl_parking_lots)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::parkingSpacesAsMarkerArray(parking_spaces, cl_parking_spaces)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::laneletsBoundaryAsMarkerArray( - shoulder_lanelets, cl_shoulder_borders, viz_lanelets_centerline_, "shoulder_")); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsBoundaryAsMarkerArray( - road_lanelets, cl_ll_borders, viz_lanelets_centerline_)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::autowareTrafficLightsAsMarkerArray(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( - road_lanelets, cl_trafficlights)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::generateTrafficLightRegulatoryElementIdMaker( - crosswalk_lanelets, cl_trafficlights)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::generateTrafficLightIdMaker(aw_tl_reg_elems, cl_trafficlights)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::generateLaneletIdMarker(shoulder_lanelets, cl_lanelet_id)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::generateLaneletIdMarker(road_lanelets, cl_lanelet_id)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::generateLaneletIdMarker( - crosswalk_lanelets, cl_lanelet_id, "crosswalk_lanelet_id")); - insertMarkerArray( - &map_marker_array, lanelet::visualization::laneletsAsTriangleMarkerArray( - "shoulder_road_lanelets", shoulder_lanelets, cl_shoulder)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::laneletsAsTriangleMarkerArray("road_lanelets", road_lanelets, cl_road)); - insertMarkerArray( - &map_marker_array, lanelet::visualization::noObstacleSegmentationAreaAsMarkerArray( - no_obstacle_segmentation_area, cl_no_obstacle_segmentation_area)); - insertMarkerArray( - &map_marker_array, - lanelet::visualization::noObstacleSegmentationAreaForRunOutAsMarkerArray( - no_obstacle_segmentation_area_for_run_out, cl_no_obstacle_segmentation_area_for_run_out)); - - insertMarkerArray( - &map_marker_array, - lanelet::visualization::hatchedRoadMarkingsAreaAsMarkerArray( - hatched_road_markings_area, cl_hatched_road_markings_area, cl_hatched_road_markings_line)); - - insertMarkerArray( - &map_marker_array, - lanelet::visualization::noParkingAreasAsMarkerArray(no_parking_reg_elems, cl_no_parking_areas)); - - insertMarkerArray( - &map_marker_array, - lanelet::visualization::lineStringsAsMarkerArray(curbstones, "curbstone", cl_curbstones, 0.2)); - - insertMarkerArray( - &map_marker_array, lanelet::visualization::intersectionAreaAsMarkerArray( - intersection_areas, cl_intersection_area)); - - pub_marker_->publish(map_marker_array); -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(Lanelet2MapVisualizationNode) diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp deleted file mode 100644 index a8d380fd81b86..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.cpp +++ /dev/null @@ -1,89 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "differential_map_loader_module.hpp" - -DifferentialMapLoaderModule::DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) -{ - get_differential_pcd_maps_service_ = node->create_service( - "service/get_differential_pcd_map", - std::bind( - &DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap, this, - std::placeholders::_1, std::placeholders::_2)); -} - -void DifferentialMapLoaderModule::differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const -{ - // iterate over all the available pcd map grids - std::vector should_remove(static_cast(cached_ids.size()), true); - for (const auto & ele : all_pcd_file_metadata_dict_) { - std::string path = ele.first; - PCDFileMetadata metadata = ele.second; - - // assume that the map ID = map path (for now) - std::string map_id = path; - - // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; - - auto id_in_cached_list = std::find(cached_ids.begin(), cached_ids.end(), map_id); - if (id_in_cached_list != cached_ids.end()) { - int index = id_in_cached_list - cached_ids.begin(); - should_remove[index] = false; - } else { - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); - pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; - pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; - pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; - pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y; - response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); - } - } - - for (size_t i = 0; i < cached_ids.size(); ++i) { - if (should_remove[i]) { - response->ids_to_remove.push_back(cached_ids[i]); - } - } -} - -bool DifferentialMapLoaderModule::onServiceGetDifferentialPointCloudMap( - GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res) -{ - auto area = req->area; - std::vector cached_ids = req->cached_ids; - differentialAreaLoad(area, cached_ids, res); - res->header.frame_id = "map"; - return true; -} - -autoware_map_msgs::msg::PointCloudMapCellWithID -DifferentialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const -{ - sensor_msgs::msg::PointCloud2 pcd; - if (pcl::io::loadPCDFile(path, pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; - pointcloud_map_cell_with_id.pointcloud = pcd; - pointcloud_map_cell_with_id.cell_id = map_id; - return pointcloud_map_cell_with_id; -} diff --git a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp deleted file mode 100644 index 7069e1dbdf45c..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/differential_map_loader_module.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ - -#include "utils.hpp" - -#include - -#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -class DifferentialMapLoaderModule -{ - using GetDifferentialPointCloudMap = autoware_map_msgs::srv::GetDifferentialPointCloudMap; - -public: - explicit DifferentialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); - -private: - rclcpp::Logger logger_; - - std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr get_differential_pcd_maps_service_; - - bool onServiceGetDifferentialPointCloudMap( - GetDifferentialPointCloudMap::Request::SharedPtr req, - GetDifferentialPointCloudMap::Response::SharedPtr res); - void differentialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area_info, const std::vector & cached_ids, - GetDifferentialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__DIFFERENTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp deleted file mode 100644 index 8c9378e9dfadb..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "partial_map_loader_module.hpp" - -PartialMapLoaderModule::PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) -{ - get_partial_pcd_maps_service_ = node->create_service( - "service/get_partial_pcd_map", std::bind( - &PartialMapLoaderModule::onServiceGetPartialPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); -} - -void PartialMapLoaderModule::partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const -{ - // iterate over all the available pcd map grids - for (const auto & ele : all_pcd_file_metadata_dict_) { - std::string path = ele.first; - PCDFileMetadata metadata = ele.second; - - // assume that the map ID = map path (for now) - std::string map_id = path; - - // skip if the pcd file is not within the queried area - if (!isGridWithinQueriedArea(area, metadata)) continue; - - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); - pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; - pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; - pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; - pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y; - - response->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); - } -} - -bool PartialMapLoaderModule::onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, GetPartialPointCloudMap::Response::SharedPtr res) -{ - auto area = req->area; - partialAreaLoad(area, res); - res->header.frame_id = "map"; - return true; -} - -autoware_map_msgs::msg::PointCloudMapCellWithID PartialMapLoaderModule::loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const -{ - sensor_msgs::msg::PointCloud2 pcd; - if (pcl::io::loadPCDFile(path, pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; - pointcloud_map_cell_with_id.pointcloud = pcd; - pointcloud_map_cell_with_id.cell_id = map_id; - return pointcloud_map_cell_with_id; -} diff --git a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp deleted file mode 100644 index 4d97ab90667ec..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ - -#include "utils.hpp" - -#include - -#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -class PartialMapLoaderModule -{ - using GetPartialPointCloudMap = autoware_map_msgs::srv::GetPartialPointCloudMap; - -public: - explicit PartialMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); - -private: - rclcpp::Logger logger_; - - std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr get_partial_pcd_maps_service_; - - bool onServiceGetPartialPointCloudMap( - GetPartialPointCloudMap::Request::SharedPtr req, - GetPartialPointCloudMap::Response::SharedPtr res); - void partialAreaLoad( - const autoware_map_msgs::msg::AreaInfo & area, - GetPartialPointCloudMap::Response::SharedPtr & response) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__PARTIAL_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp deleted file mode 100644 index a66f9ee99534c..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pointcloud_map_loader_module.hpp" - -#include "utils.hpp" - -#include - -#include -#include - -sensor_msgs::msg::PointCloud2 downsample( - const sensor_msgs::msg::PointCloud2 & msg_input, const float leaf_size) -{ - pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); - pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); - pcl::fromROSMsg(msg_input, *pcl_input); - pcl::VoxelGrid filter; - filter.setInputCloud(pcl_input); - filter.setLeafSize(leaf_size, leaf_size, leaf_size); - filter.filter(*pcl_output); - - sensor_msgs::msg::PointCloud2 msg_output; - pcl::toROSMsg(*pcl_output, msg_output); - msg_output.header = msg_input.header; - return msg_output; -} - -PointcloudMapLoaderModule::PointcloudMapLoaderModule( - rclcpp::Node * node, const std::vector & pcd_paths, - const std::string & publisher_name, const bool use_downsample) -: logger_(node->get_logger()) -{ - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - pub_pointcloud_map_ = - node->create_publisher(publisher_name, durable_qos); - - sensor_msgs::msg::PointCloud2 pcd; - if (use_downsample) { - const float leaf_size = node->declare_parameter("leaf_size"); - pcd = loadPCDFiles(pcd_paths, leaf_size); - } else { - pcd = loadPCDFiles(pcd_paths, boost::none); - } - - if (pcd.width == 0) { - RCLCPP_ERROR(logger_, "No PCD was loaded: pcd_paths.size() = %zu", pcd_paths.size()); - return; - } - - pcd.header.frame_id = "map"; - pub_pointcloud_map_->publish(pcd); -} - -sensor_msgs::msg::PointCloud2 PointcloudMapLoaderModule::loadPCDFiles( - const std::vector & pcd_paths, const boost::optional leaf_size) const -{ - sensor_msgs::msg::PointCloud2 whole_pcd; - sensor_msgs::msg::PointCloud2 partial_pcd; - - for (size_t i = 0; i < pcd_paths.size(); ++i) { - auto & path = pcd_paths[i]; - if (i % 50 == 0) { - RCLCPP_DEBUG_STREAM( - logger_, fmt::format("Load {} ({} out of {})", path, i + 1, pcd_paths.size())); - } - - if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - - if (leaf_size) { - partial_pcd = downsample(partial_pcd, leaf_size.get()); - } - - if (whole_pcd.width == 0) { - whole_pcd = partial_pcd; - } else { - whole_pcd.width += partial_pcd.width; - whole_pcd.row_step += partial_pcd.row_step; - whole_pcd.data.insert(whole_pcd.data.end(), partial_pcd.data.begin(), partial_pcd.data.end()); - } - } - - whole_pcd.header.frame_id = "map"; - - return whole_pcd; -} diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp deleted file mode 100644 index 6a87643b57bff..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_module.hpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ - -#include - -#include - -#include - -#include -#include -#include - -#include -#include - -class PointcloudMapLoaderModule -{ -public: - explicit PointcloudMapLoaderModule( - rclcpp::Node * node, const std::vector & pcd_paths, - const std::string & publisher_name, const bool use_downsample); - -private: - rclcpp::Logger logger_; - rclcpp::Publisher::SharedPtr pub_pointcloud_map_; - - sensor_msgs::msg::PointCloud2 loadPCDFiles( - const std::vector & pcd_paths, const boost::optional leaf_size) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp deleted file mode 100644 index bacbabe60a719..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "pointcloud_map_loader_node.hpp" - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace fs = std::filesystem; - -namespace -{ -bool isPcdFile(const std::string & p) -{ - if (fs::is_directory(p)) { - return false; - } - - const std::string ext = fs::path(p).extension(); - - if (ext != ".pcd" && ext != ".PCD") { - return false; - } - - return true; -} -} // namespace - -PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & options) -: Node("pointcloud_map_loader", options) -{ - const auto pcd_paths = - getPcdPaths(declare_parameter>("pcd_paths_or_directory")); - std::string pcd_metadata_path = declare_parameter("pcd_metadata_path"); - bool enable_whole_load = declare_parameter("enable_whole_load"); - bool enable_downsample_whole_load = declare_parameter("enable_downsampled_whole_load"); - bool enable_partial_load = declare_parameter("enable_partial_load"); - bool enable_selected_load = declare_parameter("enable_selected_load"); - - if (enable_whole_load) { - std::string publisher_name = "output/pointcloud_map"; - pcd_map_loader_ = - std::make_unique(this, pcd_paths, publisher_name, false); - } - - if (enable_downsample_whole_load) { - std::string publisher_name = "output/debug/downsampled_pointcloud_map"; - downsampled_pcd_map_loader_ = - std::make_unique(this, pcd_paths, publisher_name, true); - } - - std::map pcd_metadata_dict; - try { - pcd_metadata_dict = getPCDMetadata(pcd_metadata_path, pcd_paths); - } catch (std::runtime_error & e) { - RCLCPP_ERROR_STREAM(get_logger(), e.what()); - } - - if (enable_partial_load) { - partial_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } - - differential_map_loader_ = std::make_unique(this, pcd_metadata_dict); - - if (enable_selected_load) { - selected_map_loader_ = std::make_unique(this, pcd_metadata_dict); - } -} - -std::map PointCloudMapLoaderNode::getPCDMetadata( - const std::string & pcd_metadata_path, const std::vector & pcd_paths) const -{ - if (fs::exists(pcd_metadata_path)) { - auto pcd_metadata_dict = loadPCDMetadata(pcd_metadata_path); - pcd_metadata_dict = replaceWithAbsolutePath(pcd_metadata_dict, pcd_paths); - return pcd_metadata_dict; - } else if (pcd_paths.size() == 1) { - // An exception when using a single file PCD map so that the users do not have to provide - // a metadata file. - // Note that this should ideally be avoided and thus eventually be removed by someone, until - // Autoware users get used to handling the PCD file(s) with metadata. - RCLCPP_DEBUG_STREAM(get_logger(), "Create PCD metadata, as the pointcloud is a single file."); - pcl::PointCloud single_pcd; - const auto & pcd_path = pcd_paths.front(); - if (pcl::io::loadPCDFile(pcd_path, single_pcd) == -1) { - throw std::runtime_error("PCD load failed: " + pcd_path); - } - PCDFileMetadata metadata = {}; - pcl::getMinMax3D(single_pcd, metadata.min, metadata.max); - return std::map{{pcd_path, metadata}}; - } else { - throw std::runtime_error("PCD metadata file not found: " + pcd_metadata_path); - } -} - -std::vector PointCloudMapLoaderNode::getPcdPaths( - const std::vector & pcd_paths_or_directory) const -{ - std::vector pcd_paths; - for (const auto & p : pcd_paths_or_directory) { - if (!fs::exists(p)) { - RCLCPP_ERROR_STREAM(get_logger(), "invalid path: " << p); - } - - if (isPcdFile(p)) { - pcd_paths.push_back(p); - } - - if (fs::is_directory(p)) { - for (const auto & file : fs::directory_iterator(p)) { - const auto filename = file.path().string(); - if (isPcdFile(filename)) { - pcd_paths.push_back(filename); - } - } - } - } - return pcd_paths; -} - -#include -RCLCPP_COMPONENTS_REGISTER_NODE(PointCloudMapLoaderNode) diff --git a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp b/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp deleted file mode 100644 index fd9b297a9e3f4..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/pointcloud_map_loader_node.hpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ -#define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ - -#include "differential_map_loader_module.hpp" -#include "partial_map_loader_module.hpp" -#include "pointcloud_map_loader_module.hpp" -#include "selected_map_loader_module.hpp" - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -class PointCloudMapLoaderNode : public rclcpp::Node -{ -public: - explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); - -private: - std::unique_ptr pcd_map_loader_; - std::unique_ptr downsampled_pcd_map_loader_; - std::unique_ptr partial_map_loader_; - std::unique_ptr differential_map_loader_; - std::unique_ptr selected_map_loader_; - - std::vector getPcdPaths( - const std::vector & pcd_paths_or_directory) const; - std::map getPCDMetadata( - const std::string & pcd_metadata_path, const std::vector & pcd_paths) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp deleted file mode 100644 index 3e7b046f9d178..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.cpp +++ /dev/null @@ -1,107 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "selected_map_loader_module.hpp" -namespace -{ -autoware_map_msgs::msg::PointCloudMapMetaData createMetadata( - const std::map & pcd_file_metadata_dict) -{ - autoware_map_msgs::msg::PointCloudMapMetaData metadata_msg; - metadata_msg.header.frame_id = "map"; - metadata_msg.header.stamp = rclcpp::Clock().now(); - - for (const auto & ele : pcd_file_metadata_dict) { - std::string path = ele.first; - PCDFileMetadata metadata = ele.second; - - // assume that the map ID = map path (for now) - std::string map_id = path; - - autoware_map_msgs::msg::PointCloudMapCellMetaDataWithID cell_metadata_with_id; - cell_metadata_with_id.cell_id = map_id; - cell_metadata_with_id.metadata.min_x = metadata.min.x; - cell_metadata_with_id.metadata.min_y = metadata.min.y; - cell_metadata_with_id.metadata.max_x = metadata.max.x; - cell_metadata_with_id.metadata.max_y = metadata.max.y; - - metadata_msg.metadata_list.push_back(cell_metadata_with_id); - } - - return metadata_msg; -} -} // namespace - -SelectedMapLoaderModule::SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict) -: logger_(node->get_logger()), all_pcd_file_metadata_dict_(pcd_file_metadata_dict) -{ - get_selected_pcd_maps_service_ = node->create_service( - "service/get_selected_pcd_map", std::bind( - &SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap, - this, std::placeholders::_1, std::placeholders::_2)); - - // publish the map metadata - rclcpp::QoS durable_qos{1}; - durable_qos.transient_local(); - pub_metadata_ = node->create_publisher( - "output/pointcloud_map_metadata", durable_qos); - pub_metadata_->publish(createMetadata(all_pcd_file_metadata_dict_)); -} - -bool SelectedMapLoaderModule::onServiceGetSelectedPointCloudMap( - GetSelectedPointCloudMap::Request::SharedPtr req, - GetSelectedPointCloudMap::Response::SharedPtr res) const -{ - const auto request_ids = req->cell_ids; - for (const auto & request_id : request_ids) { - const auto requested_selected_map_iterator = all_pcd_file_metadata_dict_.find(request_id); - - // skip if the requested ID is not found - if (requested_selected_map_iterator == all_pcd_file_metadata_dict_.end()) { - RCLCPP_WARN(logger_, "ID %s not found", request_id.c_str()); - continue; - } - - const std::string path = requested_selected_map_iterator->first; - // assume that the map ID = map path (for now) - const std::string map_id = path; - PCDFileMetadata metadata = requested_selected_map_iterator->second; - - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id = - loadPointCloudMapCellWithID(path, map_id); - pointcloud_map_cell_with_id.metadata.min_x = metadata.min.x; - pointcloud_map_cell_with_id.metadata.min_y = metadata.min.y; - pointcloud_map_cell_with_id.metadata.max_x = metadata.max.x; - pointcloud_map_cell_with_id.metadata.max_y = metadata.max.y; - - res->new_pointcloud_with_ids.push_back(pointcloud_map_cell_with_id); - } - res->header.frame_id = "map"; - return true; -} - -autoware_map_msgs::msg::PointCloudMapCellWithID -SelectedMapLoaderModule::loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const -{ - sensor_msgs::msg::PointCloud2 pcd; - if (pcl::io::loadPCDFile(path, pcd) == -1) { - RCLCPP_ERROR_STREAM(logger_, "PCD load failed: " << path); - } - autoware_map_msgs::msg::PointCloudMapCellWithID pointcloud_map_cell_with_id; - pointcloud_map_cell_with_id.pointcloud = pcd; - pointcloud_map_cell_with_id.cell_id = map_id; - return pointcloud_map_cell_with_id; -} diff --git a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp b/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp deleted file mode 100644 index f44d549a0f576..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/selected_map_loader_module.hpp +++ /dev/null @@ -1,59 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__SELECTED_MAP_LOADER_MODULE_HPP_ -#define POINTCLOUD_MAP_LOADER__SELECTED_MAP_LOADER_MODULE_HPP_ - -#include "utils.hpp" - -#include - -#include "autoware_map_msgs/msg/point_cloud_map_meta_data.hpp" -#include "autoware_map_msgs/srv/get_selected_point_cloud_map.hpp" - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -class SelectedMapLoaderModule -{ - using GetSelectedPointCloudMap = autoware_map_msgs::srv::GetSelectedPointCloudMap; - -public: - explicit SelectedMapLoaderModule( - rclcpp::Node * node, const std::map & pcd_file_metadata_dict); - -private: - rclcpp::Logger logger_; - - std::map all_pcd_file_metadata_dict_; - rclcpp::Service::SharedPtr get_selected_pcd_maps_service_; - - rclcpp::Publisher::SharedPtr pub_metadata_; - - bool onServiceGetSelectedPointCloudMap( - GetSelectedPointCloudMap::Request::SharedPtr req, - GetSelectedPointCloudMap::Response::SharedPtr res) const; - autoware_map_msgs::msg::PointCloudMapCellWithID loadPointCloudMapCellWithID( - const std::string & path, const std::string & map_id) const; -}; - -#endif // POINTCLOUD_MAP_LOADER__SELECTED_MAP_LOADER_MODULE_HPP_ diff --git a/map/map_loader/src/pointcloud_map_loader/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp deleted file mode 100644 index 2430ce74e3e8b..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/utils.cpp +++ /dev/null @@ -1,101 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "utils.hpp" - -#include - -#include -#include -#include - -std::map loadPCDMetadata(const std::string & pcd_metadata_path) -{ - YAML::Node config = YAML::LoadFile(pcd_metadata_path); - - std::map metadata; - - for (const auto & node : config) { - if ( - node.first.as() == "x_resolution" || - node.first.as() == "y_resolution") { - continue; - } - - std::string key = node.first.as(); - std::vector values = node.second.as>(); - - PCDFileMetadata fileMetadata; - fileMetadata.min.x = values[0]; - fileMetadata.min.y = values[1]; - fileMetadata.max.x = values[0] + config["x_resolution"].as(); - fileMetadata.max.y = values[1] + config["y_resolution"].as(); - - metadata[key] = fileMetadata; - } - - return metadata; -} - -std::map replaceWithAbsolutePath( - const std::map & pcd_metadata_path, - const std::vector & pcd_paths) -{ - std::map absolute_path_map; - for (const auto & path : pcd_paths) { - std::string filename = path.substr(path.find_last_of("/\\") + 1); - auto it = pcd_metadata_path.find(filename); - if (it != pcd_metadata_path.end()) { - absolute_path_map[path] = it->second; - } - } - return absolute_path_map; -} - -bool cylinderAndBoxOverlapExists( - const double center_x, const double center_y, const double radius, - const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point) -{ - // Collision detection with x-y plane (circular base of the cylinder) - if ( - box_min_point.x - radius <= center_x && center_x <= box_max_point.x + radius && - box_min_point.y - radius <= center_y && center_y <= box_max_point.y + radius) { - return true; - } - - // Collision detection with box edges - const double dx0 = center_x - box_min_point.x; - const double dx1 = center_x - box_max_point.x; - const double dy0 = center_y - box_min_point.y; - const double dy1 = center_y - box_max_point.y; - - if ( - std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius || - std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius) { - return true; - } - - return false; -} - -bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) -{ - // Currently, the area load only supports cylindrical area - double center_x = area.center_x; - double center_y = area.center_y; - double radius = area.radius; - bool res = cylinderAndBoxOverlapExists(center_x, center_y, radius, metadata.min, metadata.max); - return res; -} diff --git a/map/map_loader/src/pointcloud_map_loader/utils.hpp b/map/map_loader/src/pointcloud_map_loader/utils.hpp deleted file mode 100644 index 62d0b34e516e3..0000000000000 --- a/map/map_loader/src/pointcloud_map_loader/utils.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef POINTCLOUD_MAP_LOADER__UTILS_HPP_ -#define POINTCLOUD_MAP_LOADER__UTILS_HPP_ - -#include -#include - -#include -#include - -#include -#include -#include - -struct PCDFileMetadata -{ - pcl::PointXYZ min; - pcl::PointXYZ max; - bool operator==(const PCDFileMetadata & other) const - { - return min.x == other.min.x && min.y == other.min.y && min.z == other.min.z && - max.x == other.max.x && max.y == other.max.y && max.z == other.max.z; - } -}; - -std::map loadPCDMetadata(const std::string & pcd_metadata_path); -std::map replaceWithAbsolutePath( - const std::map & pcd_metadata_path, - const std::vector & pcd_paths); - -bool cylinderAndBoxOverlapExists( - const double center_x, const double center_y, const double radius, - const pcl::PointXYZ position_min, const pcl::PointXYZ position_max); -bool isGridWithinQueriedArea( - const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata); - -#endif // POINTCLOUD_MAP_LOADER__UTILS_HPP_ diff --git a/map/map_loader/test/data/test_map.osm b/map/map_loader/test/data/test_map.osm deleted file mode 100644 index 406cd85c151ea..0000000000000 --- a/map/map_loader/test/data/test_map.osm +++ /dev/null @@ -1,146 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/map/map_loader/test/lanelet2_map_loader_launch.test.py b/map/map_loader/test/lanelet2_map_loader_launch.test.py deleted file mode 100644 index f1aa9e0efe922..0000000000000 --- a/map/map_loader/test/lanelet2_map_loader_launch.test.py +++ /dev/null @@ -1,60 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2022 TIER IV, Inc. -# -# 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. - -import os -import unittest - -from ament_index_python import get_package_share_directory -import launch -from launch import LaunchDescription -from launch_ros.actions import Node -import launch_testing -import pytest - - -@pytest.mark.launch_test -def generate_test_description(): - lanelet2_map_path = os.path.join( - get_package_share_directory("map_loader"), "test/data/test_map.osm" - ) - - lanelet2_map_loader = Node( - package="map_loader", - executable="lanelet2_map_loader", - parameters=[{"lanelet2_map_path": lanelet2_map_path, "center_line_resolution": 5.0}], - ) - - context = {} - - return ( - LaunchDescription( - [ - lanelet2_map_loader, - # Start test after 1s - gives time for the map_loader to finish initialization - launch.actions.TimerAction( - period=1.0, actions=[launch_testing.actions.ReadyToTest()] - ), - ] - ), - context, - ) - - -@launch_testing.post_shutdown_test() -class TestProcessOutput(unittest.TestCase): - def test_exit_code(self, proc_info): - # Check that process exits with code 0: no error - launch_testing.asserts.assertExitCodes(proc_info) diff --git a/map/map_loader/test/test_cylinder_box_overlap.cpp b/map/map_loader/test/test_cylinder_box_overlap.cpp deleted file mode 100644 index 4c9b18a3dbd8e..0000000000000 --- a/map/map_loader/test/test_cylinder_box_overlap.cpp +++ /dev/null @@ -1,84 +0,0 @@ -// Copyright 2023 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 "../src/pointcloud_map_loader/utils.hpp" - -#include - -TEST(CylinderAndBoxOverlapExists, NoOverlap) -{ - // Cylinder: center = (5, 0), radius = 4 - 0.1 - // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) - double center_x = 5.0; - double center_y = 0.0; - - const double radius = 4.0 - 0.1; - - pcl::PointXYZ p_min; - p_min.x = -1.0; - p_min.y = -1.0; - p_min.z = -1.0; - - pcl::PointXYZ p_max; - p_max.x = 1.0; - p_max.y = 1.0; - p_max.z = 1.0; - - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); - EXPECT_FALSE(result); -} - -TEST(CylinderAndBoxOverlapExists, Overlap1) -{ - // Cylinder: center = (0, 0), radius = 5 - // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) - double center_x = 0.0; - double center_y = 0.0; - const double radius = 5.0; - - pcl::PointXYZ p_min; - p_min.x = -1.0; - p_min.y = -1.0; - p_min.z = -1.0; - - pcl::PointXYZ p_max; - p_max.x = 1.0; - p_max.y = 1.0; - p_max.z = 1.0; - - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); - EXPECT_TRUE(result); -} - -TEST(CylinderAndBoxOverlapExists, Overlap2) -{ - // Cylinder: center = (0, 0), radius = 5 - // Box: p_min = (-1, -1, -100), p_max = (1, 1, -99) - double center_x = 0.0; - double center_y = 0.0; - const double radius = 5.0; - - pcl::PointXYZ p_min; - p_min.x = -1.0; - p_min.y = -1.0; - p_min.z = -100.0; - - pcl::PointXYZ p_max; - p_max.x = 1.0; - p_max.y = 1.0; - p_max.z = -99.0; - - bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max); - EXPECT_TRUE(result); -} diff --git a/map/map_loader/test/test_differential_map_loader_module.cpp b/map/map_loader/test/test_differential_map_loader_module.cpp deleted file mode 100644 index e25a8f97a70ca..0000000000000 --- a/map/map_loader/test/test_differential_map_loader_module.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright 2023 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 "../src/pointcloud_map_loader/differential_map_loader_module.hpp" - -#include - -#include "autoware_map_msgs/srv/get_differential_point_cloud_map.hpp" - -#include -#include - -#include - -using autoware_map_msgs::srv::GetDifferentialPointCloudMap; - -class TestDifferentialMapLoaderModule : public ::testing::Test -{ -protected: - void SetUp() override - { - // Initialize ROS node - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_differential_map_loader_module"); - - // Generate a sample dummy pointcloud and save it to a file - pcl::PointCloud dummy_cloud; - dummy_cloud.width = 3; - dummy_cloud.height = 1; - dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); - dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); - dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); - dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); - pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); - - // Generate a sample dummy pointcloud metadata dictionary - std::map dummy_metadata_dict; - PCDFileMetadata dummy_metadata; - dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); - dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); - dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; - - // Initialize the DifferentialMapLoaderModule with the dummy metadata dictionary - module_ = std::make_shared(node_.get(), dummy_metadata_dict); - - // Create a client for the GetDifferentialPointCloudMap service - client_ = - node_->create_client("service/get_differential_pcd_map"); - } - - void TearDown() override { rclcpp::shutdown(); } - - rclcpp::Node::SharedPtr node_; - std::shared_ptr module_; - rclcpp::Client::SharedPtr client_; -}; - -TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles) -{ - // Wait for the GetDifferentialPointCloudMap service to be available - ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); - - // Prepare a request for the service - auto request = std::make_shared(); - request->area.center_x = 0; - request->area.center_y = 0; - request->area.radius = 2; - request->cached_ids.clear(); - - // Call the service - auto result_future = client_->async_send_request(request); - ASSERT_EQ( - rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); - - // Check the result - auto result = result_future.get(); - ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); - EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); - EXPECT_EQ(static_cast(result->ids_to_remove.size()), 0); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/map/map_loader/test/test_load_pcd_metadata.cpp b/map/map_loader/test/test_load_pcd_metadata.cpp deleted file mode 100644 index a832489b6db99..0000000000000 --- a/map/map_loader/test/test_load_pcd_metadata.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "../src/pointcloud_map_loader/utils.hpp" - -#include - -#include -#include - -using ::testing::ContainerEq; - -std::string createYAMLFile() -{ - std::filesystem::path tmp_path = std::filesystem::temp_directory_path() / "temp_metadata.yaml"; - - std::ofstream ofs(tmp_path); - ofs << "file1.pcd: [1, 2]\n"; - ofs << "file2.pcd: [3, 4]\n"; - ofs << "x_resolution: 5\n"; - ofs << "y_resolution: 6\n"; - ofs.close(); - - return tmp_path.string(); -} - -TEST(LoadPCDMetadataTest, BasicFunctionality) -{ - std::string yaml_file_path = createYAMLFile(); - - std::map expected = { - {"file1.pcd", {{1, 2, 0}, {6, 8, 0}}}, - {"file2.pcd", {{3, 4, 0}, {8, 10, 0}}}, - }; - - auto result = loadPCDMetadata(yaml_file_path); - ASSERT_THAT(result, ContainerEq(expected)); -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleMock(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/map/map_loader/test/test_partial_map_loader_module.cpp b/map/map_loader/test/test_partial_map_loader_module.cpp deleted file mode 100644 index 9ff27df29ab8b..0000000000000 --- a/map/map_loader/test/test_partial_map_loader_module.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// Copyright 2023 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 "../src/pointcloud_map_loader/partial_map_loader_module.hpp" - -#include - -#include "autoware_map_msgs/srv/get_partial_point_cloud_map.hpp" - -#include - -#include - -using autoware_map_msgs::srv::GetPartialPointCloudMap; - -class TestPartialMapLoaderModule : public ::testing::Test -{ -protected: - void SetUp() override - { - // Initialize ROS node - rclcpp::init(0, nullptr); - node_ = std::make_shared("test_partial_map_loader_module"); - - // Generate a sample dummy pointcloud and save it to a file - pcl::PointCloud dummy_cloud; - dummy_cloud.width = 3; - dummy_cloud.height = 1; - dummy_cloud.points.resize(dummy_cloud.width * dummy_cloud.height); - dummy_cloud.points[0] = pcl::PointXYZ(-1.0, -1.0, -1.0); - dummy_cloud.points[1] = pcl::PointXYZ(0.0, 0.0, 0.0); - dummy_cloud.points[2] = pcl::PointXYZ(1.0, 1.0, 1.0); - pcl::io::savePCDFileASCII("/tmp/dummy.pcd", dummy_cloud); - - // Generate a sample dummy pointcloud metadata dictionary - std::map dummy_metadata_dict; - PCDFileMetadata dummy_metadata; - dummy_metadata.min = pcl::PointXYZ(-1.0, -1.0, -1.0); - dummy_metadata.max = pcl::PointXYZ(1.0, 1.0, 1.0); - dummy_metadata_dict["/tmp/dummy.pcd"] = dummy_metadata; - - // Initialize the PartialMapLoaderModule with the dummy metadata dictionary - module_ = std::make_shared(node_.get(), dummy_metadata_dict); - - // Create a client for the GetPartialPointCloudMap service - client_ = node_->create_client("service/get_partial_pcd_map"); - } - - void TearDown() override { rclcpp::shutdown(); } - - rclcpp::Node::SharedPtr node_; - std::shared_ptr module_; - rclcpp::Client::SharedPtr client_; -}; - -TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles) -{ - // Wait for the GetPartialPointCloudMap service to be available - ASSERT_TRUE(client_->wait_for_service(std::chrono::seconds(3))); - - // Prepare a request for the service - auto request = std::make_shared(); - request->area.center_x = 0; - request->area.center_y = 0; - request->area.radius = 2; - - // Call the service - auto result_future = client_->async_send_request(request); - ASSERT_EQ( - rclcpp::spin_until_future_complete(node_, result_future), rclcpp::FutureReturnCode::SUCCESS); - - // Check the result - auto result = result_future.get(); - ASSERT_EQ(static_cast(result->new_pointcloud_with_ids.size()), 1); - EXPECT_EQ(result->new_pointcloud_with_ids[0].cell_id, "/tmp/dummy.pcd"); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/map/map_loader/test/test_pointcloud_map_loader_module.cpp b/map/map_loader/test/test_pointcloud_map_loader_module.cpp deleted file mode 100644 index 2b686dc0fe8c3..0000000000000 --- a/map/map_loader/test/test_pointcloud_map_loader_module.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2023 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 "../src/pointcloud_map_loader/pointcloud_map_loader_module.hpp" -#include "../src/pointcloud_map_loader/utils.hpp" - -#include - -#include - -#include -#include -#include - -#include -#include - -using std::chrono_literals::operator""ms; - -class TestPointcloudMapLoaderModule : public ::testing::Test -{ -protected: - rclcpp::Node::SharedPtr node; - std::string temp_pcd_path; - - void SetUp() override - { - rclcpp::init(0, nullptr); - node = rclcpp::Node::make_shared("test_pointcloud_map_loader_module"); - - // Create a temporary PCD file with dummy point cloud data - pcl::PointCloud cloud; - cloud.width = 5; - cloud.height = 1; - cloud.is_dense = false; - cloud.points.resize(cloud.width * cloud.height); - - for (size_t i = 0; i < cloud.points.size(); ++i) { - cloud.points[i].x = static_cast(i); - cloud.points[i].y = static_cast(i * 2); - cloud.points[i].z = static_cast(i * 3); - } - - temp_pcd_path = "/tmp/test_pointcloud_map_loader_module.pcd"; - pcl::io::savePCDFileASCII(temp_pcd_path, cloud); - } - - void TearDown() override { rclcpp::shutdown(); } -}; - -TEST_F(TestPointcloudMapLoaderModule, LoadPCDFilesNoDownsampleTest) -{ - // Prepare PCD paths - std::vector pcd_paths = {temp_pcd_path}; - - // Create PointcloudMapLoaderModule instance - PointcloudMapLoaderModule loader(node.get(), pcd_paths, "pointcloud_map_no_downsample", false); - - // Create a subscriber to the published pointcloud topic - auto pointcloud_received = std::make_shared(false); - auto pointcloud_msg = std::make_shared(); - - rclcpp::QoS durable_qos{10}; - durable_qos.transient_local(); // Match publisher's Durability setting - - auto pointcloud_sub = node->create_subscription( - "pointcloud_map_no_downsample", durable_qos, - [pointcloud_received, pointcloud_msg](const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) { - *pointcloud_received = true; - *pointcloud_msg = *msg; - }); - - // Spin until pointcloud is received or timeout occurs - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - auto start_time = node->now(); - while (!*pointcloud_received && (node->now() - start_time).seconds() < 3) { - executor.spin_some(50ms); - } - - // Check if the point cloud is received and the content is as expected - ASSERT_TRUE(*pointcloud_received); - - // Convert the received point cloud to pcl::PointCloud - pcl::PointCloud received_cloud; - pcl::fromROSMsg(*pointcloud_msg, received_cloud); - - // Check if the received point cloud matches the content of the temporary PCD file - ASSERT_EQ(static_cast(received_cloud.width), 5); - ASSERT_EQ(static_cast(received_cloud.height), 1); - - for (size_t i = 0; i < received_cloud.points.size(); ++i) { - EXPECT_FLOAT_EQ(received_cloud.points[i].x, static_cast(i)); - EXPECT_FLOAT_EQ(received_cloud.points[i].y, static_cast(i * 2)); - EXPECT_FLOAT_EQ(received_cloud.points[i].z, static_cast(i * 3)); - } -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/map/map_loader/test/test_replace_with_absolute_path.cpp b/map/map_loader/test/test_replace_with_absolute_path.cpp deleted file mode 100644 index 10680e05103ce..0000000000000 --- a/map/map_loader/test/test_replace_with_absolute_path.cpp +++ /dev/null @@ -1,65 +0,0 @@ -// Copyright 2022 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "../src/pointcloud_map_loader/utils.hpp" - -#include -#include - -using ::testing::ContainerEq; - -TEST(ReplaceWithAbsolutePathTest, BasicFunctionality) -{ - std::map pcd_metadata_path = { - {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, - {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, - }; - - std::vector pcd_paths = { - "/home/user/pcd/file1.pcd", - "/home/user/pcd/file2.pcd", - }; - - std::map expected = { - {"/home/user/pcd/file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, - {"/home/user/pcd/file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, - }; - - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); - ASSERT_THAT(result, ContainerEq(expected)); -} - -TEST(ReplaceWithAbsolutePathTest, NoMatchingFiles) -{ - std::map pcd_metadata_path = { - {"file1.pcd", {{1, 2, 3}, {4, 5, 6}}}, - {"file2.pcd", {{-1, -2, -3}, {-4, -5, -6}}}, - }; - - std::vector pcd_paths = { - "/home/user/pcd/non_matching_file1.pcd", - "/home/user/pcd/non_matching_file2.pcd", - }; - - std::map expected = {}; - - auto result = replaceWithAbsolutePath(pcd_metadata_path, pcd_paths); - ASSERT_THAT(result, ContainerEq(expected)); -} - -int main(int argc, char ** argv) -{ - ::testing::InitGoogleMock(&argc, argv); - return RUN_ALL_TESTS(); -}