From a015692056cea8ccf7fa3c1fa061019a56c8e116 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 1 Dec 2022 11:16:22 +0900 Subject: [PATCH] feat(map_loader): add partial map loading interface in pointcloud_map_loader (#1938) * first commit Signed-off-by: kminoda * reverted unnecessary modification Signed-off-by: kminoda * ci(pre-commit): autofix * renamed some classes Signed-off-by: kminoda * ci(pre-commit): autofix * move autoware_map_msgs to autoware_msgs repos Signed-off-by: kminoda * catch up with the modification in autoware_map_msgs Signed-off-by: kminoda * ci(pre-commit): autofix * aligned with autoware_map_msgs change (differential/partial modules seperation) Signed-off-by: kminoda * ci(pre-commit): autofix * debugged * debugged Signed-off-by: kminoda * added min-max info and others Signed-off-by: kminoda * ci(pre-commit): autofix * minor fix Signed-off-by: kminoda * already_loaded -> cached Signed-off-by: kminoda * ci(pre-commit): autofix * load_ -> get_ Signed-off-by: kminoda * ci(pre-commit): autofix * resolve pre-commit Signed-off-by: kminoda * ci(pre-commit): autofix * minor fix Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * ci(pre-commit): autofix * update readme Signed-off-by: kminoda * minor fix in readme Signed-off-by: kminoda * grammarly Signed-off-by: kminoda * ci(pre-commit): autofix * ci(pre-commit): autofix * fix copyright Signed-off-by: kminoda * fix launch file Signed-off-by: kminoda * remove leaf_size param Signed-off-by: kminoda * removed unnecessary things Signed-off-by: kminoda * removed downsample for now Signed-off-by: kminoda * removed differential_map_loader for this PR (would make another PR for this) Signed-off-by: kminoda * ci(pre-commit): autofix * removed differential_map_loader, debugged Signed-off-by: kminoda * ci(pre-commit): autofix * removed leaf_size description Signed-off-by: kminoda * ci(pre-commit): autofix * refactor sphereAndBoxOverlapExists Signed-off-by: kminoda * ci(pre-commit): autofix * added test for sphereAndBoxOverlapExists Signed-off-by: kminoda * ci(pre-commit): autofix * remove downsample function for now Signed-off-by: kminoda * remove fmt from target_link_libraries in test Signed-off-by: kminoda * minor fix in cmakelists.txt Signed-off-by: kminoda Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- launch/tier4_map_launch/CMakeLists.txt | 1 + .../config/pointcloud_map_loader.param.yaml | 4 + launch/tier4_map_launch/launch/map.launch.py | 22 ++- launch/tier4_map_launch/launch/map.launch.xml | 9 +- map/map_loader/CMakeLists.txt | 2 + map/map_loader/README.md | 29 +++- .../config/pointcloud_map_loader.param.yaml | 4 + .../launch/pointcloud_map_loader.launch.xml | 3 + map/map_loader/package.xml | 3 + .../partial_map_loader_module.cpp | 69 +++++++++ .../partial_map_loader_module.hpp | 59 ++++++++ .../pointcloud_map_loader_module.cpp | 1 - .../pointcloud_map_loader_node.cpp | 30 +++- .../pointcloud_map_loader_node.hpp | 7 + .../src/pointcloud_map_loader/utils.cpp | 72 +++++++++ .../src/pointcloud_map_loader/utils.hpp | 35 +++++ .../test/test_sphere_box_overlap.cpp | 142 ++++++++++++++++++ 17 files changed, 478 insertions(+), 14 deletions(-) create mode 100644 launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml create mode 100644 map/map_loader/config/pointcloud_map_loader.param.yaml create mode 100644 map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp create mode 100644 map/map_loader/src/pointcloud_map_loader/utils.cpp create mode 100644 map/map_loader/src/pointcloud_map_loader/utils.hpp create mode 100644 map/map_loader/test/test_sphere_box_overlap.cpp diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index 7ad8b620591e..2e18cad4de3e 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -19,4 +19,5 @@ endif() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml new file mode 100644 index 000000000000..76026167f81a --- /dev/null +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + enable_whole_load: true + enable_partial_load: true diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index de74108cf0a1..69cdf7b7ed9a 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -32,9 +32,14 @@ def launch_setup(context, *args, **kwargs): lanelet2_map_loader_param_path = LaunchConfiguration("lanelet2_map_loader_param_path").perform( context ) + pointcloud_map_loader_param_path = LaunchConfiguration( + "pointcloud_map_loader_param_path" + ).perform(context) with open(lanelet2_map_loader_param_path, "r") as f: lanelet2_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(pointcloud_map_loader_param_path, "r") as f: + pointcloud_map_loader_param = yaml.safe_load(f)["/**"]["ros__parameters"] map_hash_generator = Node( package="map_loader", @@ -77,9 +82,14 @@ def launch_setup(context, *args, **kwargs): package="map_loader", plugin="PointCloudMapLoaderNode", name="pointcloud_map_loader", - remappings=[("output/pointcloud_map", "pointcloud_map")], + remappings=[ + ("output/pointcloud_map", "pointcloud_map"), + ("service/get_partial_pcd_map", "/map/get_partial_pointcloud_map"), + ("service/get_differential_pcd_map", "/map/get_differential_pointcloud_map"), + ], parameters=[ - {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]} + {"pcd_paths_or_directory": ["[", LaunchConfiguration("pointcloud_map_path"), "]"]}, + pointcloud_map_loader_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -149,6 +159,14 @@ def add_launch_arg(name: str, default_value=None, description=None): ], "path to lanelet2_map_loader param file", ), + add_launch_arg( + "pointcloud_map_loader_param_path", + [ + FindPackageShare("tier4_map_launch"), + "/config/pointcloud_map_loader.param.yaml", + ], + "path to pointcloud_map_loader param file", + ), add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), add_launch_arg("use_multithread", "false", "use multithread"), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 233e340dcde8..282e862a1dff 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -2,6 +2,7 @@ + @@ -9,10 +10,10 @@ - - - - + + + + diff --git a/map/map_loader/CMakeLists.txt b/map/map_loader/CMakeLists.txt index bd0155cdc610..de3480b68bc9 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -19,6 +19,8 @@ ament_auto_find_build_dependencies() 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/utils.cpp ) target_link_libraries(pointcloud_map_loader_node ${PCL_LIBRARIES}) diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 90c1e8891a18..baafe9729387 100644 --- a/map/map_loader/README.md +++ b/map/map_loader/README.md @@ -6,15 +6,34 @@ This package provides the features of loading various maps. ### Feature -pointcloud_map_loader loads PointCloud file and publishes the map data as sensor_msgs/PointCloud2 message. +`pointcloud_map_loader` provides pointcloud maps to the other Autoware nodes in various configurations. +Currently, it supports the following two types: -### How to run +- Publish raw pointcloud map +- Send partial pointcloud map loading via ROS 2 service -`ros2 run map_loader pointcloud_map_loader --ros-args -p "pcd_paths_or_directory:=[path/to/pointcloud1.pcd, path/to/pointcloud2.pcd, ...]"` +#### Publish raw pointcloud map (ROS 2 topic) -### Published Topics +The node publishes the raw pointcloud map loaded from the `.pcd` file(s). + +#### 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. + +### Parameters + +| Name | Type | Description | Default value | +| :------------------ | :--- | :--------------------------------------------- | :------------ | +| enable_whole_load | bool | A flag to enable raw pointcloud map publishing | true | +| enable_partial_load | bool | A flag to enable partial pointcloud map server | true | + +### Interfaces -- pointcloud_map (sensor_msgs/PointCloud2) : PointCloud Map +- `output/pointcloud_map` (sensor_msgs/msg/PointCloud2) : Raw pointcloud map +- `service/get_partial_pcd_map` (autoware_map_msgs/srv/GetPartialPointCloudMap) : Partial pointcloud map --- diff --git a/map/map_loader/config/pointcloud_map_loader.param.yaml b/map/map_loader/config/pointcloud_map_loader.param.yaml new file mode 100644 index 000000000000..76026167f81a --- /dev/null +++ b/map/map_loader/config/pointcloud_map_loader.param.yaml @@ -0,0 +1,4 @@ +/**: + ros__parameters: + enable_whole_load: true + enable_partial_load: true diff --git a/map/map_loader/launch/pointcloud_map_loader.launch.xml b/map/map_loader/launch/pointcloud_map_loader.launch.xml index 43676570277b..3cfc457c15d2 100644 --- a/map/map_loader/launch/pointcloud_map_loader.launch.xml +++ b/map/map_loader/launch/pointcloud_map_loader.launch.xml @@ -1,8 +1,11 @@ + + + diff --git a/map/map_loader/package.xml b/map/map_loader/package.xml index 4618d134844c..6342e09d7103 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -11,11 +11,13 @@ ament_cmake_auto autoware_auto_mapping_msgs + autoware_map_msgs fmt geometry_msgs lanelet2_extension libpcl-all-dev pcl_conversions + pcl_ros rclcpp rclcpp_components std_msgs @@ -24,6 +26,7 @@ tier4_autoware_utils visualization_msgs + ament_cmake_gmock ament_lint_auto autoware_lint_common 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 new file mode 100644 index 000000000000..af53438e8e32 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.cpp @@ -0,0 +1,69 @@ +// 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); + 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 new file mode 100644 index 000000000000..052aa4245c09 --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/partial_map_loader_module.hpp @@ -0,0 +1,59 @@ +// 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 index 0d75cd9dab01..e83b0df4d21c 100644 --- 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 @@ -15,7 +15,6 @@ #include "pointcloud_map_loader_module.hpp" #include -#include #include #include 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 index d9b1336ca33a..e6a93694737c 100644 --- 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 @@ -15,6 +15,7 @@ #include "pointcloud_map_loader_node.hpp" #include +#include #include #include @@ -48,9 +49,18 @@ PointCloudMapLoaderNode::PointCloudMapLoaderNode(const rclcpp::NodeOptions & opt { const auto pcd_paths = getPcdPaths(declare_parameter>("pcd_paths_or_directory")); + bool enable_whole_load = declare_parameter("enable_whole_load"); + bool enable_partial_load = declare_parameter("enable_partial_load"); - std::string publisher_name = "output/pointcloud_map"; - pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); + if (enable_whole_load) { + std::string publisher_name = "output/pointcloud_map"; + pcd_map_loader_ = std::make_unique(this, pcd_paths, publisher_name); + } + + if (enable_partial_load) { + pcd_metadata_dict_ = generatePCDMetadata(pcd_paths); + partial_map_loader_ = std::make_unique(this, pcd_metadata_dict_); + } } std::vector PointCloudMapLoaderNode::getPcdPaths( @@ -78,5 +88,21 @@ std::vector PointCloudMapLoaderNode::getPcdPaths( return pcd_paths; } +std::map PointCloudMapLoaderNode::generatePCDMetadata( + const std::vector & pcd_paths) const +{ + pcl::PointCloud partial_pcd; + std::map all_pcd_file_metadata_dict; + for (const auto & path : pcd_paths) { + if (pcl::io::loadPCDFile(path, partial_pcd) == -1) { + RCLCPP_ERROR_STREAM(get_logger(), "PCD load failed: " << path); + } + PCDFileMetadata metadata = {}; + pcl::getMinMax3D(partial_pcd, metadata.min, metadata.max); + all_pcd_file_metadata_dict[path] = metadata; + } + return all_pcd_file_metadata_dict; +} + #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 index 598665a84cd3..7a297c900be1 100644 --- 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 @@ -15,6 +15,7 @@ #ifndef POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ #define POINTCLOUD_MAP_LOADER__POINTCLOUD_MAP_LOADER_NODE_HPP_ +#include "partial_map_loader_module.hpp" #include "pointcloud_map_loader_module.hpp" #include @@ -36,10 +37,16 @@ class PointCloudMapLoaderNode : public rclcpp::Node explicit PointCloudMapLoaderNode(const rclcpp::NodeOptions & options); private: + // ros param + std::map pcd_metadata_dict_; + std::unique_ptr pcd_map_loader_; + std::unique_ptr partial_map_loader_; std::vector getPcdPaths( const std::vector & pcd_paths_or_directory) const; + std::map generatePCDMetadata( + 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/utils.cpp b/map/map_loader/src/pointcloud_map_loader/utils.cpp new file mode 100644 index 000000000000..8af8c0dfd8ec --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/utils.cpp @@ -0,0 +1,72 @@ +// 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 + +bool sphereAndBoxOverlapExists( + const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point, + const pcl::PointXYZ box_max_point) +{ + // Collision detection with x-axis plane + if ( + box_min_point.x - radius <= center.x && center.x <= box_max_point.x + radius && + box_min_point.y <= center.y && center.y <= box_max_point.y && box_min_point.z <= center.z && + center.z <= box_max_point.z) { + return true; + } + + // Collision detection with y-axis plane + if ( + box_min_point.x <= center.x && center.x <= box_max_point.x && + box_min_point.y - radius <= center.y && center.y <= box_max_point.y + radius && + box_min_point.z <= center.z && center.z <= box_max_point.z) { + return true; + } + + // Collision detection with z-axis plane + if ( + box_min_point.x <= center.x && center.x <= box_max_point.x && box_min_point.y <= center.y && + center.y <= box_max_point.y && box_min_point.z - radius <= center.z && + center.z <= box_max_point.z + 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; + const double dz0 = center.z - box_min_point.z; + const double dz1 = center.z - box_max_point.z; + if ( + std::hypot(dx0, dy0, dz0) <= radius || std::hypot(dx1, dy0, dz0) <= radius || + std::hypot(dx0, dy1, dz0) <= radius || std::hypot(dx0, dy0, dz1) <= radius || + std::hypot(dx0, dy1, dz1) <= radius || std::hypot(dx1, dy0, dz1) <= radius || + std::hypot(dx1, dy1, dz0) <= radius || std::hypot(dx1, dy1, dz1) <= radius) { + return true; + } + return false; +} + +bool isGridWithinQueriedArea( + const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata) +{ + // Currently, the area load only supports spherical area + geometry_msgs::msg::Point center = area.center; + double radius = area.radius; + bool res = sphereAndBoxOverlapExists(center, 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 new file mode 100644 index 000000000000..0a0d2912bb8c --- /dev/null +++ b/map/map_loader/src/pointcloud_map_loader/utils.hpp @@ -0,0 +1,35 @@ +// 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 + +struct PCDFileMetadata +{ + pcl::PointXYZ min; + pcl::PointXYZ max; +}; + +bool sphereAndBoxOverlapExists( + const geometry_msgs::msg::Point position, 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/test_sphere_box_overlap.cpp b/map/map_loader/test/test_sphere_box_overlap.cpp new file mode 100644 index 000000000000..f7f76cc74007 --- /dev/null +++ b/map/map_loader/test/test_sphere_box_overlap.cpp @@ -0,0 +1,142 @@ +// 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 + +TEST(SphereAndBoxOverlapExists, NoOverlap1) +{ + // Sphere: center = (5, 0, 0), radius = 4 - 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 5.0; + center.y = 0.0; + center.z = 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 = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_FALSE(result); +} + +TEST(SphereAndBoxOverlapExists, NoOverlap2) +{ + // Sphere: center = (2, 2, 0), radius = sqrt(2) - 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 2.0; + center.y = 2.0; + center.z = 0.0; + + const double radius = std::sqrt(2) - 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 = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_FALSE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap1) +{ + // Sphere: center = (0, 0, 0), radius = 0.5 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = 0.0; + + const double radius = 0.5; + + 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 = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap2) +{ + // Sphere: center = (0, 0, 5), radius = 4 + 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 0.0; + center.y = 0.0; + center.z = 5.0; + + const double radius = 4 + 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 = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +} + +TEST(SphereAndBoxOverlapExists, Overlap3) +{ + // Sphere: center = (2, 2, 2), radius = sqrt(3) + 0.1 + // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1) + geometry_msgs::msg::Point center; + center.x = 2.0; + center.y = 2.0; + center.z = 2.0; + + const double radius = std::sqrt(3) + 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 = sphereAndBoxOverlapExists(center, radius, p_min, p_max); + EXPECT_TRUE(result); +}