diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index bd636b289ca93..d3735e5509a94 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -38,6 +38,12 @@ def __init__(self, context): self.use_down_sample_filter = self.pointcloud_map_filter_param["use_down_sample_filter"] self.voxel_size = self.pointcloud_map_filter_param["down_sample_voxel_size"] self.distance_threshold = self.pointcloud_map_filter_param["distance_threshold"] + self.timer_interval_ms = self.pointcloud_map_filter_param["timer_interval_ms"] + self.use_dynamic_map_loading = self.pointcloud_map_filter_param["use_dynamic_map_loading"] + self.map_update_distance_threshold = self.pointcloud_map_filter_param[ + "map_update_distance_threshold" + ] + self.map_loader_radius = self.pointcloud_map_filter_param["map_loader_radius"] def create_pipeline(self): if self.use_down_sample_filter: @@ -56,10 +62,17 @@ def create_normal_pipeline(self): ("input", LaunchConfiguration("input_topic")), ("map", "/map/pointcloud_map"), ("output", LaunchConfiguration("output_topic")), + ("map_loader_service", "/map/get_differential_pointcloud_map"), + ("pose_with_covariance", "/localization/pose_estimator/pose_with_covariance"), ], parameters=[ { "distance_threshold": self.distance_threshold, + "timer_interval_ms": self.timer_interval_ms, + "use_dynamic_map_loading": self.use_dynamic_map_loading, + "map_update_distance_threshold": self.map_update_distance_threshold, + "map_loader_radius": self.map_loader_radius, + "input_frame": "map", } ], extra_arguments=[ diff --git a/perception/compare_map_segmentation/CMakeLists.txt b/perception/compare_map_segmentation/CMakeLists.txt index 5cc1f7bad104b..5b0ae044b1b12 100644 --- a/perception/compare_map_segmentation/CMakeLists.txt +++ b/perception/compare_map_segmentation/CMakeLists.txt @@ -27,6 +27,7 @@ add_library(compare_map_segmentation SHARED src/voxel_based_compare_map_filter_nodelet.cpp src/voxel_distance_based_compare_map_filter_nodelet.cpp src/compare_elevation_map_filter_node.cpp + src/voxel_grid_map_loader.cpp ) target_link_libraries(compare_map_segmentation @@ -44,6 +45,7 @@ ament_target_dependencies(compare_map_segmentation rclcpp_components sensor_msgs tier4_autoware_utils + autoware_map_msgs ) if(OPENMP_FOUND) diff --git a/perception/compare_map_segmentation/README.md b/perception/compare_map_segmentation/README.md index 6cd652cce5582..a304fcb67e555 100644 --- a/perception/compare_map_segmentation/README.md +++ b/perception/compare_map_segmentation/README.md @@ -2,7 +2,7 @@ ## Purpose -The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map). +The `compare_map_segmentation` is a node that filters the ground points from the input pointcloud by using map info (e.g. pcd, elevation map or split map pointcloud from map_loader interface). ## Inner-workings / Algorithms @@ -24,7 +24,9 @@ WIP ### Voxel Based Compare Map Filter -WIP +The filter loads the map pointcloud (static loading whole map at once at beginning or dynamic loading during vehicle moving) and utilizes VoxelGrid to downsample map pointcloud. + +For each point of input pointcloud, the filter use `getCentroidIndexAt` combine with `getGridCoordinates` function from VoxelGrid class to check if the downsampled map point existing surrounding input points. Remove the input point which has downsampled map point in voxels containing or being close to the point. ### Voxel Distance based Compare Map Filter @@ -47,14 +49,23 @@ WIP | ----------------- | ------------------------------- | --------------- | | `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | +#### Parameters + +| Name | Type | Description | Default value | +| :------------------- | :----- | :------------------------------------------------------------------------------ | :------------ | +| `map_layer_name` | string | elevation map layer name | elevation | +| `map_frame` | float | frame_id of the map that is temporarily used before elevation_map is subscribed | map | +| `height_diff_thresh` | float | Remove points whose height difference is below this value [m] | 0.15 | + ### Other Filters #### Input -| Name | Type | Description | -| ---------------- | ------------------------------- | ---------------- | -| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | -| `~/input/map` | `grid_map::msg::GridMap` | map | +| Name | Type | Description | +| ------------------------ | ----------------------------------------------- | ------------------------------------------------------ | +| `~/input/points` | `sensor_msgs::msg::PointCloud2` | reference points | +| `~/input/map` | `sensor_msgs::msg::PointCloud2` | map (in case static map loading) | +| `~/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | current ego-vehicle pose (in case dynamic map loading) | #### Output @@ -62,15 +73,15 @@ WIP | ----------------- | ------------------------------- | --------------- | | `~/output/points` | `sensor_msgs::msg::PointCloud2` | filtered points | -## Parameters - -### Core Parameters +#### Parameters -| Name | Type | Description | Default value | -| :------------------- | :----- | :------------------------------------------------------------------------------ | :------------ | -| `map_layer_name` | string | elevation map layer name | elevation | -| `map_frame` | float | frame_id of the map that is temporarily used before elevation_map is subscribed | map | -| `height_diff_thresh` | float | Remove points whose height difference is below this value [m] | 0.15 | +| Name | Type | Description | Default value | +| :------------------------------ | :---- | :------------------------------------------------------------------------------------------- | :------------ | +| `use_dynamic_map_loading` | bool | map loading mode selection, `true` for dynamic map loading, `false` for static map loading | true | +| `distance_threshold` | float | VoxelGrid's leaf_size also the threshold to check distance between input point and map point | 0.5 | +| `map_update_distance_threshold` | float | Threshold of vehicle movement distance when map update is necessary [m] | 10.0 | +| `map_loader_radius` | float | Radius of map need to be loaded (in dynamic map loading) [m] | 150.0 | +| `timer_interval_ms` | int | time interval of timer to check if update the map is necessary (in dynamic map loading) [ms] | 100 | ## Assumptions / Known limits diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp new file mode 100644 index 0000000000000..b8daedbcab903 --- /dev/null +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/multi_voxel_grid_map_update.hpp @@ -0,0 +1,317 @@ +// Copyright 2023 Autoware Foundation +// +// 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 COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ +#define COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ + +#include + +#include + +#include +#include +#include +#include +#include + +#include // for FLT_MAX +#include +#include +#include +#include +#include +#include + +namespace compare_map_segmentation +{ +template +// TODO(badai-nguyen): when map loader I/F is updated, remove this class since +// boundary point calculation become unnecessary +class MultiVoxelGrid : public pcl::VoxelGrid +{ + typedef std::map> MapCellDict; + struct cloud_point_index_idx + { + unsigned int idx; + unsigned int cloud_point_index; + + cloud_point_index_idx() = default; + cloud_point_index_idx(unsigned int idx_, unsigned int cloud_point_index_) + : idx(idx_), cloud_point_index(cloud_point_index_) + { + } + bool operator<(const cloud_point_index_idx & p) const { return (idx < p.idx); } + }; + +protected: + using pcl::VoxelGrid::filter_name_; + using pcl::VoxelGrid::downsample_all_data_; + using pcl::VoxelGrid::input_; + using pcl::VoxelGrid::save_leaf_layout_; + using pcl::VoxelGrid::min_b_; + using pcl::VoxelGrid::max_b_; + using pcl::VoxelGrid::divb_mul_; + using pcl::VoxelGrid::div_b_; + using pcl::VoxelGrid::inverse_leaf_size_; + + using pcl::VoxelGrid::filter_limit_negative_; + using pcl::VoxelGrid::filter_limit_max_; + using pcl::VoxelGrid::filter_limit_min_; + using pcl::VoxelGrid::indices_; + using pcl::VoxelGrid::min_points_per_voxel_; + using pcl::VoxelGrid::filter_field_name_; + + using PointCloud = typename pcl::Filter::PointCloud; + using PointCloudPtr = typename PointCloud::Ptr; + using PointCloudConstPtr = typename PointCloud::ConstPtr; + +public: + using pcl::VoxelGrid::leaf_layout_; + + inline void set_voxel_grid( + std::vector * leaf_layout, const Eigen::Vector4i & min_b, const Eigen::Vector4i & max_b, + const Eigen::Vector4i & div_b, const Eigen::Vector4i & divb_mul, + const Eigen::Array4f & inverse_leaf_size) + { + leaf_layout_ = std::move(*leaf_layout); + min_b_ = min_b; + max_b_ = max_b; + div_b_ = div_b; + divb_mul_ = divb_mul; + inverse_leaf_size_ = inverse_leaf_size; + } + inline Eigen::Vector4f get_min_p() const { return min_p; } + inline Eigen::Vector4f get_max_p() const { return max_p; } + inline Eigen::Vector4i get_min_b() const { return min_b_; } + inline Eigen::Vector4i get_divb_mul() const { return divb_mul_; } + inline Eigen::Vector4i get_max_b() const { return max_b_; } + inline Eigen::Vector4i get_div_b() const { return div_b_; } + inline Eigen::Array4f get_inverse_leaf_size() const { return inverse_leaf_size_; } + inline std::vector getLeafLayout() { return (leaf_layout_); } + + // TODO(badai-nguyen): when map loader I/F is updated, use default Voxel applyFilter since + // boundary point calculation become unnecessary + inline void applyFilter(PointCloud & output) override + { + // Has the input dataset been set already? + if (!input_) { + // pcl::PCL_WARN ("[pcl::applyFilter] No input dataset given!\n"); + output.width = output.height = 0; + output.clear(); + return; + } + + // Copy the header (and thus the frame_id) + allocate enough space for points + output.height = 1; // downsampling breaks the organized structure + output.is_dense = true; // we filter out invalid points + + // Get the minimum and maximum dimensions + if (!filter_field_name_.empty()) // If we don't want to process the entire cloud... + pcl::getMinMax3D( + input_, *indices_, filter_field_name_, static_cast(filter_limit_min_), + static_cast(filter_limit_max_), min_p, max_p, filter_limit_negative_); + else + pcl::getMinMax3D(*input_, *indices_, min_p, max_p); + + // Check that the leaf size is not too small, given the size of the data + std::int64_t dx = static_cast((max_p[0] - min_p[0]) * inverse_leaf_size_[0]) + 1; + std::int64_t dy = static_cast((max_p[1] - min_p[1]) * inverse_leaf_size_[1]) + 1; + std::int64_t dz = static_cast((max_p[2] - min_p[2]) * inverse_leaf_size_[2]) + 1; + + if ((dx * dy * dz) > static_cast(std::numeric_limits::max())) { + // pcl::PCL_WARN("[pcl::applyFilter] Leaf size is too small for the input dataset. Integer + // indices would overflow.\n"); + output = *input_; + return; + } + + // Compute the minimum and maximum bounding box values + min_b_[0] = static_cast(std::floor(min_p[0] * inverse_leaf_size_[0])); + max_b_[0] = static_cast(std::floor(max_p[0] * inverse_leaf_size_[0])); + min_b_[1] = static_cast(std::floor(min_p[1] * inverse_leaf_size_[1])); + max_b_[1] = static_cast(std::floor(max_p[1] * inverse_leaf_size_[1])); + min_b_[2] = static_cast(std::floor(min_p[2] * inverse_leaf_size_[2])); + max_b_[2] = static_cast(std::floor(max_p[2] * inverse_leaf_size_[2])); + + // Compute the number of divisions needed along all axis + div_b_ = max_b_ - min_b_ + Eigen::Vector4i::Ones(); + div_b_[3] = 0; + + // Set up the division multiplier + divb_mul_ = Eigen::Vector4i(1, div_b_[0], div_b_[0] * div_b_[1], 0); + + // Storage for mapping leaf and pointcloud indexes + std::vector index_vector; + index_vector.reserve(indices_->size()); + + // If we don't want to process the entire cloud, but rather filter points far away from the + // viewpoint first... + if (!filter_field_name_.empty()) { + // Get the distance field index + std::vector fields; + int distance_idx = pcl::getFieldIndex(filter_field_name_, fields); + if (distance_idx == -1) + + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (const auto & index : (*indices_)) { + if (!input_->is_dense) + // Check if the point is invalid + if (!pcl::isXYZFinite((*input_)[index])) continue; + + // Get the distance value + const std::uint8_t * pt_data = reinterpret_cast(&(*input_)[index]); + float distance_value = 0; + memcpy(&distance_value, pt_data + fields[distance_idx].offset, sizeof(float)); + + if (filter_limit_negative_) { + // Use a threshold for cutting out points which inside the interval + if ((distance_value < filter_limit_max_) && (distance_value > filter_limit_min_)) + continue; + } else { + // Use a threshold for cutting out points which are too close/far away + if ((distance_value > filter_limit_max_) || (distance_value < filter_limit_min_)) + continue; + } + + int ijk0 = static_cast( + std::floor((*input_)[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + std::floor((*input_)[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + std::floor((*input_)[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back(static_cast(idx), index); + } + } else { + // No distance filtering, process all data + // First pass: go over all points and insert them into the index_vector vector + // with calculated idx. Points with the same idx value will contribute to the + // same point of resulting CloudPoint + for (const auto & index : (*indices_)) { + if (!input_->is_dense) + // Check if the point is invalid + if (!pcl::isXYZFinite((*input_)[index])) continue; + + int ijk0 = static_cast( + std::floor((*input_)[index].x * inverse_leaf_size_[0]) - static_cast(min_b_[0])); + int ijk1 = static_cast( + std::floor((*input_)[index].y * inverse_leaf_size_[1]) - static_cast(min_b_[1])); + int ijk2 = static_cast( + std::floor((*input_)[index].z * inverse_leaf_size_[2]) - static_cast(min_b_[2])); + + // Compute the centroid leaf index + int idx = ijk0 * divb_mul_[0] + ijk1 * divb_mul_[1] + ijk2 * divb_mul_[2]; + index_vector.emplace_back(static_cast(idx), index); + } + } + + // Second pass: sort the index_vector vector using value representing target cell as index + // in effect all points belonging to the same output cell will be next to each other + auto rightshift_func = [](const cloud_point_index_idx & x, const unsigned offset) { + return x.idx >> offset; + }; + boost::sort::spreadsort::integer_sort( + index_vector.begin(), index_vector.end(), rightshift_func); + + // Third pass: count output cells + // we need to skip all the same, adjacent idx values + unsigned int total = 0; + unsigned int index = 0; + // first_and_last_indices_vector[i] represents the index in index_vector of the first point in + // index_vector belonging to the voxel which corresponds to the i-th output point, + // and of the first point not belonging to. + std::vector> first_and_last_indices_vector; + // Worst case size + first_and_last_indices_vector.reserve(index_vector.size()); + while (index < index_vector.size()) { + unsigned int i = index + 1; + while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx) ++i; + if (i - index >= min_points_per_voxel_) { + ++total; + first_and_last_indices_vector.emplace_back(index, i); + } + index = i; + } + + // Fourth pass: compute centroids, insert them into their final position + output.resize(total); + if (save_leaf_layout_) { + try { + // Resizing won't reset old elements to -1. If leaf_layout_ has been used previously, it + // needs to be re-initialized to -1 + std::uint32_t new_layout_size = div_b_[0] * div_b_[1] * div_b_[2]; + // This is the number of elements that need to be re-initialized to -1 + std::uint32_t reinit_size = std::min( + static_cast(new_layout_size), + static_cast(leaf_layout_.size())); + for (std::uint32_t i = 0; i < reinit_size; i++) { + leaf_layout_[i] = -1; + } + leaf_layout_.resize(new_layout_size, -1); + } catch (std::bad_alloc &) { + throw pcl::PCLException( + "VoxelGrid bin size is too low; impossible to allocate memory for layout", + "voxel_grid.hpp", "applyFilter"); + } catch (std::length_error &) { + throw pcl::PCLException( + "VoxelGrid bin size is too low; impossible to allocate memory for layout", + "voxel_grid.hpp", "applyFilter"); + } + } + + index = 0; + for (const auto & cp : first_and_last_indices_vector) { + // calculate centroid - sum values from all input points, that have the same idx value in + // index_vector array + unsigned int first_index = cp.first; + unsigned int last_index = cp.second; + + // index is centroid final position in resulting PointCloud + if (save_leaf_layout_) leaf_layout_[index_vector[first_index].idx] = index; + + // Limit downsampling to coords + if (!downsample_all_data_) { + Eigen::Vector4f centroid(Eigen::Vector4f::Zero()); + + for (unsigned int li = first_index; li < last_index; ++li) + centroid += (*input_)[index_vector[li].cloud_point_index].getVector4fMap(); + + centroid /= static_cast(last_index - first_index); + output[index].getVector4fMap() = centroid; + } else { + pcl::CentroidPoint centroid; + + // fill in the accumulator with leaf points + for (unsigned int li = first_index; li < last_index; ++li) + centroid.add((*input_)[index_vector[li].cloud_point_index]); + + centroid.get(output[index]); + } + + ++index; + } + output.width = output.size(); + } + +private: + Eigen::Vector4f min_p, max_p; +}; + +}; // namespace compare_map_segmentation + +#endif // COMPARE_MAP_SEGMENTATION__MULTI_VOXEL_GRID_MAP_UPDATE_HPP_ diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp index 6f2b2f90ab1e1..692193d13b04f 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_compare_map_filter_nodelet.hpp @@ -15,11 +15,13 @@ #ifndef COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ #define COMPARE_MAP_SEGMENTATION__VOXEL_BASED_COMPARE_MAP_FILTER_NODELET_HPP_ +#include "compare_map_segmentation/voxel_grid_map_loader.hpp" #include "pointcloud_preprocessor/filter.hpp" #include #include +#include #include namespace compare_map_segmentation @@ -30,25 +32,14 @@ class VoxelBasedCompareMapFilterComponent : public pointcloud_preprocessor::Filt virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); - void input_target_callback(const PointCloud2ConstPtr map); - bool is_in_voxel( - const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, - /* Can not add const in PCL specification */ pcl::VoxelGrid & voxel) const; - private: // pcl::SegmentDifferences impl_; + std::unique_ptr voxel_grid_map_loader_; rclcpp::Subscription::SharedPtr sub_map_; - PointCloudPtr voxel_map_ptr_; double distance_threshold_; - pcl::VoxelGrid voxel_grid_; bool set_map_in_voxel_grid_; - /** \brief Parameter service callback result : needed to be hold */ - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - - /** \brief Parameter service callback */ - rcl_interfaces::msg::SetParametersResult paramCallback(const std::vector & p); + bool dynamic_map_load_enable_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp new file mode 100644 index 0000000000000..f01e01edd1a27 --- /dev/null +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp @@ -0,0 +1,210 @@ +// Copyright 2023 Autoware Foundation +// +// 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 COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ +#define COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ + +#include "compare_map_segmentation/multi_voxel_grid_map_update.hpp" + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +// create map loader interface for static and dynamic map + +template +double distance3D(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + double dz = p1.z - p2.z; + return dx * dx + dy * dy + dz * dz; +} + +template +double distance2D(const T p1, const U p2) +{ + double dx = p1.x - p2.x; + double dy = p1.y - p2.y; + return std::sqrt(dx * dx + dy * dy); +} + +class VoxelGridMapLoader +{ +protected: + rclcpp::Logger logger_; + std::mutex * mutex_ptr_; + double voxel_leaf_size_; + rclcpp::Publisher::SharedPtr downsampled_map_pub_; + +public: + typedef compare_map_segmentation::MultiVoxelGrid MultiVoxelGrid; + typedef typename pcl::Filter::PointCloud PointCloud; + typedef typename PointCloud::Ptr PointCloudPtr; + explicit VoxelGridMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; + bool is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, + MultiVoxelGrid & voxel) const; + bool is_in_voxel( + const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, + const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const; + + void publish_downsampled_map(const pcl::PointCloud & downsampled_pc); + bool is_close_points( + const pcl::PointXYZ point, const pcl::PointXYZ target_point, + const double distance_threshold) const; + std::string * tf_map_input_frame_; +}; + +//*************************** for Static Map loader Voxel Grid Filter ************* +class VoxelGridStaticMapLoader : public VoxelGridMapLoader +{ +private: + rclcpp::Subscription::SharedPtr sub_map_; + MultiVoxelGrid voxel_grid_; + PointCloudPtr voxel_map_ptr_; + +public: + explicit VoxelGridStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex); + void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); +}; + +// *************** for Dynamic and Differential Map loader Voxel Grid Filter ************* +class VoxelGridDynamicMapLoader : public VoxelGridMapLoader +{ + struct MapGridVoxelInfo + { + MultiVoxelGrid map_cell_voxel_grid; + PointCloudPtr map_cell_pc_ptr; + float min_b_x, min_b_y, max_b_x, max_b_y; + }; + + typedef typename std::map VoxelGridDict; + +private: + VoxelGridDict current_voxel_grid_dict_; + rclcpp::Subscription::SharedPtr + sub_estimated_pose_; + std::optional current_position_ = std::nullopt; + std::optional last_updated_position_ = std::nullopt; + rclcpp::TimerBase::SharedPtr map_update_timer_; + double map_update_distance_threshold_; + double map_loader_radius_; + rclcpp::Client::SharedPtr + map_update_client_; + rclcpp::CallbackGroup::SharedPtr client_callback_group_; + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; + +public: + explicit VoxelGridDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group); + void onEstimatedPoseCallback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr pose); + + void timer_callback(); + bool should_update_map() const; + void request_update_map(const geometry_msgs::msg::Point & position); + bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); + + inline pcl::PointCloud getCurrentDownsampledMapPc() const + { + pcl::PointCloud output; + for (const auto & kv : current_voxel_grid_dict_) { + output = output + *(kv.second.map_cell_pc_ptr); + } + return output; + } + inline std::vector getCurrentMapIDs() const + { + std::vector current_map_ids{}; + for (auto & kv : current_voxel_grid_dict_) { + current_map_ids.push_back(kv.first); + } + return current_map_ids; + } + inline void updateDifferentialMapCells( + const std::vector & map_cells_to_add, + std::vector map_cell_ids_to_remove) + { + for (const auto & map_cell_to_add : map_cells_to_add) { + addMapCellAndFilter(map_cell_to_add); + } + for (size_t i = 0; i < map_cell_ids_to_remove.size(); ++i) { + removeMapCell(map_cell_ids_to_remove.at(i)); + } + } + + inline void removeMapCell(const std::string map_cell_id_to_remove) + { + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.erase(map_cell_id_to_remove); + (*mutex_ptr_).unlock(); + } + + inline void addMapCellAndFilter( + const autoware_map_msgs::msg::PointCloudMapCellWithID & map_cell_to_add) + { + pcl::PointCloud map_cell_pc_tmp; + pcl::fromROSMsg(map_cell_to_add.pointcloud, map_cell_pc_tmp); + + MultiVoxelGrid map_cell_voxel_grid_tmp; + PointCloudPtr map_cell_downsampled_pc_ptr_tmp; + + auto map_cell_voxel_input_tmp_ptr = + std::make_shared>(map_cell_pc_tmp); + map_cell_voxel_grid_tmp.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + map_cell_downsampled_pc_ptr_tmp.reset(new pcl::PointCloud); + map_cell_voxel_grid_tmp.setInputCloud(map_cell_voxel_input_tmp_ptr); + map_cell_voxel_grid_tmp.setSaveLeafLayout(true); + map_cell_voxel_grid_tmp.filter(*map_cell_downsampled_pc_ptr_tmp); + + MapGridVoxelInfo current_voxel_grid_list_item; + // TODO(badai-nguyen): use map cell info from map cell, when map loader I/F is updated + current_voxel_grid_list_item.min_b_x = map_cell_voxel_grid_tmp.get_min_p()[0]; + current_voxel_grid_list_item.min_b_y = map_cell_voxel_grid_tmp.get_min_p()[1]; + current_voxel_grid_list_item.max_b_x = map_cell_voxel_grid_tmp.get_max_p()[0]; + current_voxel_grid_list_item.max_b_y = map_cell_voxel_grid_tmp.get_max_p()[1]; + + current_voxel_grid_list_item.map_cell_voxel_grid.set_voxel_grid( + &(map_cell_voxel_grid_tmp.leaf_layout_), map_cell_voxel_grid_tmp.get_min_b(), + map_cell_voxel_grid_tmp.get_max_b(), map_cell_voxel_grid_tmp.get_div_b(), + map_cell_voxel_grid_tmp.get_divb_mul(), map_cell_voxel_grid_tmp.get_inverse_leaf_size()); + + current_voxel_grid_list_item.map_cell_pc_ptr.reset(new pcl::PointCloud); + // for (size_t i = 0; i < map_cell_downsampled_pc_ptr_tmp->points.size(); ++i) { + // current_voxel_grid_list_item.map_cell_pc_ptr->points.push_back( + // map_cell_downsampled_pc_ptr_tmp->points.at(i)); + // } + current_voxel_grid_list_item.map_cell_pc_ptr = std::move(map_cell_downsampled_pc_ptr_tmp); + // add + (*mutex_ptr_).lock(); + current_voxel_grid_dict_.insert({map_cell_to_add.cell_id, current_voxel_grid_list_item}); + (*mutex_ptr_).unlock(); + } +}; + +#endif // COMPARE_MAP_SEGMENTATION__VOXEL_GRID_MAP_LOADER_HPP_ diff --git a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml index a9f1e18a02df7..b2d86800fe435 100644 --- a/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml +++ b/perception/compare_map_segmentation/launch/voxel_based_compare_map_filter.launch.xml @@ -1,13 +1,23 @@ + + + + + + + + + + diff --git a/perception/compare_map_segmentation/package.xml b/perception/compare_map_segmentation/package.xml index ff4a17e837513..6db3f0eb3f6a8 100644 --- a/perception/compare_map_segmentation/package.xml +++ b/perception/compare_map_segmentation/package.xml @@ -17,6 +17,7 @@ autoware_cmake + autoware_map_msgs grid_map_pcl grid_map_ros pcl_conversions diff --git a/perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp b/perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp new file mode 100644 index 0000000000000..0efdf9a8a1850 --- /dev/null +++ b/perception/compare_map_segmentation/src/multi_voxel_grid_map_update.cpp @@ -0,0 +1,17 @@ +// Copyright 2023 Autoware Foundation +// +// 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 + +template class compare_map_segmentation::MultiVoxelGrid; diff --git a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp index c1462e849a2f9..cfce105bc909a 100644 --- a/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp @@ -38,17 +38,20 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( stop_watch_ptr_->tic("processing_time"); } - distance_threshold_ = static_cast(declare_parameter("distance_threshold", 0.3)); + distance_threshold_ = declare_parameter("distance_threshold"); + bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); set_map_in_voxel_grid_ = false; - - using std::placeholders::_1; - sub_map_ = this->create_subscription( - "map", rclcpp::QoS{1}.transient_local(), - std::bind(&VoxelBasedCompareMapFilterComponent::input_target_callback, this, _1)); - - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&VoxelBasedCompareMapFilterComponent::paramCallback, this, _1)); + if (use_dynamic_map_loading) { + rclcpp::CallbackGroup::SharedPtr main_callback_group; + main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + voxel_grid_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_, main_callback_group); + } else { + voxel_grid_map_loader_ = std::make_unique( + this, distance_threshold_, &tf_input_frame_, &mutex_); + } + tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_); } void VoxelBasedCompareMapFilterComponent::filter( @@ -57,171 +60,16 @@ void VoxelBasedCompareMapFilterComponent::filter( { std::scoped_lock lock(mutex_); stop_watch_ptr_->toc("processing_time", true); - if (voxel_map_ptr_ == NULL) { - output = *input; - return; - } pcl::PointCloud::Ptr pcl_input(new pcl::PointCloud); pcl::PointCloud::Ptr pcl_output(new pcl::PointCloud); pcl::fromROSMsg(*input, *pcl_input); pcl_output->points.reserve(pcl_input->points.size()); for (size_t i = 0; i < pcl_input->points.size(); ++i) { const pcl::PointXYZ point = pcl_input->points.at(i); - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z), point, distance_threshold_, voxel_map_ptr_, - voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold_, point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold_, point.z), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y - distance_threshold_, point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z - distance_threshold_), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y, point.z + distance_threshold_), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold_, point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold_, point.z), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x, point.y + distance_threshold_, point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - - if (is_in_voxel( - pcl::PointXYZ( - point.x - distance_threshold_, point.y - distance_threshold_, - point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold_, point.y - distance_threshold_, point.z), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x - distance_threshold_, point.y - distance_threshold_, - point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold_, point.y, point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold_, point.y, point.z), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold_, point.y, point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x - distance_threshold_, point.y + distance_threshold_, - point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x - distance_threshold_, point.y + distance_threshold_, point.z), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x - distance_threshold_, point.y + distance_threshold_, - point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { + if (voxel_grid_map_loader_->is_close_to_map(point, distance_threshold_)) { continue; } - - if (is_in_voxel( - pcl::PointXYZ( - point.x + distance_threshold_, point.y - distance_threshold_, - point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold_, point.y - distance_threshold_, point.z), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x + distance_threshold_, point.y - distance_threshold_, - point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold_, point.y, point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold_, point.y, point.z), point, - distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold_, point.y, point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x + distance_threshold_, point.y + distance_threshold_, - point.z - distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ(point.x + distance_threshold_, point.y + distance_threshold_, point.z), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - if (is_in_voxel( - pcl::PointXYZ( - point.x + distance_threshold_, point.y + distance_threshold_, - point.z + distance_threshold_), - point, distance_threshold_, voxel_map_ptr_, voxel_grid_)) { - continue; - } - - pcl_output->points.push_back(pcl_input->points.at(i)); + pcl_output->points.push_back(point); } pcl::toROSMsg(*pcl_output, output); output.header = input->header; @@ -237,61 +85,6 @@ void VoxelBasedCompareMapFilterComponent::filter( } } -bool VoxelBasedCompareMapFilterComponent::is_in_voxel( - const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, - const double distance_threshold, const PointCloudPtr & map, - pcl::VoxelGrid & voxel) const -{ - int voxel_index = - voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); - if (voxel_index != -1) { // not empty voxel - const double dist_x = map->points.at(voxel_index).x - target_point.x; - const double dist_y = map->points.at(voxel_index).y - target_point.y; - const double dist_z = map->points.at(voxel_index).z - target_point.z; - const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; - if (sqr_distance < distance_threshold * distance_threshold) { - return true; - } - } - return false; -} - -void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud2ConstPtr map) -{ - pcl::PointCloud map_pcl; - pcl::fromROSMsg(*map, map_pcl); - const auto map_pcl_ptr = pcl::make_shared>(map_pcl); - - std::scoped_lock lock(mutex_); - set_map_in_voxel_grid_ = true; - tf_input_frame_ = map_pcl_ptr->header.frame_id; - voxel_map_ptr_.reset(new pcl::PointCloud); - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setInputCloud(map_pcl_ptr); - voxel_grid_.setSaveLeafLayout(true); - voxel_grid_.filter(*voxel_map_ptr_); -} - -rcl_interfaces::msg::SetParametersResult VoxelBasedCompareMapFilterComponent::paramCallback( - const std::vector & p) -{ - std::scoped_lock lock(mutex_); - - if (get_param(p, "distance_threshold", distance_threshold_)) { - voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_); - voxel_grid_.setSaveLeafLayout(true); - if (set_map_in_voxel_grid_) { - voxel_grid_.filter(*voxel_map_ptr_); - } - RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_); - } - - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - return result; -} } // namespace compare_map_segmentation #include diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp new file mode 100644 index 0000000000000..d5d820c5a0b31 --- /dev/null +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -0,0 +1,388 @@ +// Copyright 2023 Autoware Foundation +// +// 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 "compare_map_segmentation/voxel_grid_map_loader.hpp" + +VoxelGridMapLoader::VoxelGridMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) +: logger_(node->get_logger()), voxel_leaf_size_(leaf_size) +{ + tf_map_input_frame_ = tf_map_input_frame; + mutex_ptr_ = mutex; + + downsampled_map_pub_ = node->create_publisher( + "debug/downsampled_map/pointcloud", rclcpp::QoS{1}.transient_local()); +} + +bool VoxelGridMapLoader::is_close_points( + const pcl::PointXYZ point, const pcl::PointXYZ target_point, + const double distance_threshold) const +{ + if (distance3D(point, target_point) < distance_threshold * distance_threshold) { + return true; + } + return false; +} + +void VoxelGridMapLoader::publish_downsampled_map( + const pcl::PointCloud & downsampled_pc) +{ + sensor_msgs::msg::PointCloud2 downsampled_map_msg; + pcl::toROSMsg(downsampled_pc, downsampled_map_msg); + downsampled_map_msg.header.frame_id = "map"; + downsampled_map_pub_->publish(downsampled_map_msg); +} + +bool VoxelGridMapLoader::is_close_to_neighbor_voxels( + const pcl::PointXYZ & point, const double distance_threshold, const PointCloudPtr & map, + MultiVoxelGrid & voxel) const +{ + // check map downsampled pc + if (map == NULL) { + return false; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y, point.z), point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z - distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y - distance_threshold, point.z + distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y, point.z - distance_threshold), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y, point.z + distance_threshold), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z - distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x, point.y + distance_threshold, point.z + distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + + if (is_in_voxel( + pcl::PointXYZ( + point.x - distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x - distance_threshold, point.y - distance_threshold, point.z), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x - distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z - distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x - distance_threshold, point.y, point.z + distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x - distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x - distance_threshold, point.y + distance_threshold, point.z), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x - distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + + if (is_in_voxel( + pcl::PointXYZ( + point.x + distance_threshold, point.y - distance_threshold, point.z - distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x + distance_threshold, point.y - distance_threshold, point.z), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x + distance_threshold, point.y - distance_threshold, point.z + distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z - distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z), point, distance_threshold, + map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x + distance_threshold, point.y, point.z + distance_threshold), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x + distance_threshold, point.y + distance_threshold, point.z - distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ(point.x + distance_threshold, point.y + distance_threshold, point.z), point, + distance_threshold, map, voxel)) { + return true; + } + if (is_in_voxel( + pcl::PointXYZ( + point.x + distance_threshold, point.y + distance_threshold, point.z + distance_threshold), + point, distance_threshold, map, voxel)) { + return true; + } + + return false; +} + +bool VoxelGridMapLoader::is_in_voxel( + const pcl::PointXYZ & src_point, const pcl::PointXYZ & target_point, + const double distance_threshold, const PointCloudPtr & map, MultiVoxelGrid & voxel) const +{ + int voxel_index = + voxel.getCentroidIndexAt(voxel.getGridCoordinates(src_point.x, src_point.y, src_point.z)); + if (voxel_index != -1) { // not empty voxel + const double dist_x = map->points.at(voxel_index).x - target_point.x; + const double dist_y = map->points.at(voxel_index).y - target_point.y; + const double dist_z = map->points.at(voxel_index).z - target_point.z; + const double sqr_distance = dist_x * dist_x + dist_y * dist_y + dist_z * dist_z; + if (sqr_distance < distance_threshold * distance_threshold) { + return true; + } + } + return false; +} + +//*************************** for Static Map loader Voxel Grid Filter ************* + +VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) +: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) +{ + sub_map_ = node->create_subscription( + "map", rclcpp::QoS{1}.transient_local(), + std::bind(&VoxelGridStaticMapLoader::onMapCallback, this, std::placeholders::_1)); + RCLCPP_INFO(logger_, "VoxelGridStaticMapLoader initialized.\n"); +} + +void VoxelGridStaticMapLoader::onMapCallback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr map) +{ + pcl::PointCloud map_pcl; + pcl::fromROSMsg(*map, map_pcl); + const auto map_pcl_ptr = pcl::make_shared>(map_pcl); + *tf_map_input_frame_ = map_pcl_ptr->header.frame_id; + (*mutex_ptr_).lock(); + voxel_map_ptr_.reset(new pcl::PointCloud); + voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_); + voxel_grid_.setInputCloud(map_pcl_ptr); + voxel_grid_.setSaveLeafLayout(true); + voxel_grid_.filter(*voxel_map_ptr_); + (*mutex_ptr_).unlock(); + publish_downsampled_map(*voxel_map_ptr_); +} +bool VoxelGridStaticMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_map_ptr_, voxel_grid_)) { + return true; + } + return false; +} +//*************** for Dynamic and Differential Map loader Voxel Grid Filter ************* + +VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( + rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, + rclcpp::CallbackGroup::SharedPtr main_callback_group) +: VoxelGridMapLoader(node, leaf_size, tf_map_input_frame, mutex) +{ + auto timer_interval_ms = node->declare_parameter("timer_interval_ms"); + map_update_distance_threshold_ = node->declare_parameter("map_update_distance_threshold"); + map_loader_radius_ = node->declare_parameter("map_loader_radius"); + auto main_sub_opt = rclcpp::SubscriptionOptions(); + main_sub_opt.callback_group = main_callback_group; + + sub_estimated_pose_ = node->create_subscription( + "pose_with_covariance", rclcpp::QoS{1}, + std::bind(&VoxelGridDynamicMapLoader::onEstimatedPoseCallback, this, std::placeholders::_1), + main_sub_opt); + RCLCPP_INFO(logger_, "VoxelGridDynamicMapLoader initialized.\n"); + + client_callback_group_ = + node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + map_update_client_ = node->create_client( + "map_loader_service", rmw_qos_profile_services_default, client_callback_group_); + + while (!map_update_client_->wait_for_service(std::chrono::seconds(1)) && rclcpp::ok()) { + RCLCPP_INFO(logger_, "service not available, waiting again ..."); + } + + timer_callback_group_ = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + map_update_timer_ = node->create_wall_timer( + std::chrono::milliseconds(timer_interval_ms), + std::bind(&VoxelGridDynamicMapLoader::timer_callback, this), timer_callback_group_); +} + +void VoxelGridDynamicMapLoader::onEstimatedPoseCallback( + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + current_position_ = msg->pose.pose.position; +} + +bool VoxelGridDynamicMapLoader::is_close_to_map( + const pcl::PointXYZ & point, const double distance_threshold) +{ + if (current_voxel_grid_dict_.size() == 0) { + return false; + } + std::vector neighbor_map_cells_id; + + for (auto & kv : current_voxel_grid_dict_) { + if (point.x < kv.second.min_b_x - distance_threshold) { + continue; + } + if (point.y < kv.second.min_b_y - distance_threshold) { + continue; + } + if (point.x > kv.second.max_b_x + distance_threshold) { + continue; + } + if (point.y > kv.second.max_b_y + distance_threshold) { + continue; + } + // the map cell is found + neighbor_map_cells_id.push_back(kv.first); + // check distance + if (kv.second.map_cell_pc_ptr == NULL) { + continue; + } + if (is_close_to_neighbor_voxels( + point, distance_threshold, kv.second.map_cell_pc_ptr, kv.second.map_cell_voxel_grid)) { + return true; + } + } + return false; +} +void VoxelGridDynamicMapLoader::timer_callback() +{ + if (current_position_ == std::nullopt) { + return; + } + if (last_updated_position_ == std::nullopt) { + request_update_map(current_position_.value()); + last_updated_position_ = current_position_; + return; + } + + if (should_update_map()) { + last_updated_position_ = current_position_; + request_update_map((current_position_.value())); + last_updated_position_ = current_position_; + } +} + +bool VoxelGridDynamicMapLoader::should_update_map() const +{ + if ( + distance2D(current_position_.value(), last_updated_position_.value()) > + map_update_distance_threshold_) { + return true; + } + return false; +} + +void VoxelGridDynamicMapLoader::request_update_map(const geometry_msgs::msg::Point & position) +{ + auto request = std::make_shared(); + request->area.center = position; + request->area.radius = map_loader_radius_; + request->cached_ids = getCurrentMapIDs(); + + auto result{map_update_client_->async_send_request( + request, + [](rclcpp::Client::SharedFuture) {})}; + + std::future_status status = result.wait_for(std::chrono::seconds(0)); + while (status != std::future_status::ready) { + RCLCPP_INFO(logger_, "Waiting for response...\n"); + if (!rclcpp::ok()) { + return; + } + status = result.wait_for(std::chrono::seconds(1)); + } + + // + if (status == std::future_status::ready) { + if ( + result.get()->new_pointcloud_with_ids.size() > 0 || result.get()->ids_to_remove.size() > 0) { + updateDifferentialMapCells( + result.get()->new_pointcloud_with_ids, result.get()->ids_to_remove); + } + } + publish_downsampled_map(getCurrentDownsampledMapPc()); +}