diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index 98f12b64f3ca4..b0e770d7c7619 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -7,4 +7,5 @@ autoware_package() 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 0000000000000..76026167f81a7 --- /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 6d5265f5945e9..bf396921fcb7a 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 7dbdd216f9625..8fe3f141a0dbd 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 cbea47bc98bde..6bfcffde71d60 100644 --- a/map/map_loader/CMakeLists.txt +++ b/map/map_loader/CMakeLists.txt @@ -9,6 +9,8 @@ find_package(PCL REQUIRED COMPONENTS io) 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}) @@ -49,6 +51,19 @@ if(BUILD_TESTING) 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}) + 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_sphere_box_overlap.cpp) endif() install(PROGRAMS diff --git a/map/map_loader/README.md b/map/map_loader/README.md index 90c1e8891a180..baafe9729387e 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 0000000000000..76026167f81a7 --- /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 43676570277b6..3cfc457c15d22 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 80392bed3350e..7a22a369744ab 100644 --- a/map/map_loader/package.xml +++ b/map/map_loader/package.xml @@ -15,11 +15,13 @@ autoware_cmake autoware_auto_mapping_msgs + autoware_map_msgs fmt geometry_msgs lanelet2_extension libpcl-all-dev pcl_conversions + pcl_ros rclcpp rclcpp_components std_msgs @@ -28,6 +30,7 @@ tier4_autoware_utils visualization_msgs + ament_cmake_gmock ament_lint_auto autoware_lint_common ros_testing 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 0000000000000..af53438e8e321 --- /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 0000000000000..052aa4245c099 --- /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 0d75cd9dab01c..e83b0df4d21c6 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 d9b1336ca33a7..e6a93694737cf 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 598665a84cd3f..7a297c900be12 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 0000000000000..8af8c0dfd8ec0 --- /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 0000000000000..0a0d2912bb8c5 --- /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 0000000000000..f7f76cc74007d --- /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); +}