From e0de9bb7f18218a98c49fe3ab3c1cb6f4d0af504 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 16 Feb 2024 20:09:22 +0900 Subject: [PATCH 1/2] feat: add voxel height offset parameter Signed-off-by: yoshiri --- ...istance_based_compare_map_filter_nodelet.hpp | 5 +++-- ...d_approximate_compare_map_filter_nodelet.hpp | 4 ++-- ...istance_based_compare_map_filter_nodelet.hpp | 10 ++++++---- .../voxel_grid_map_loader.hpp | 7 ++++--- .../voxel_based_compare_map_filter_nodelet.cpp | 8 +++++--- ...istance_based_compare_map_filter_nodelet.cpp | 8 +++++--- .../src/voxel_grid_map_loader.cpp | 17 ++++++++++------- 7 files changed, 35 insertions(+), 24 deletions(-) diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp index 301dc75839f8e..ab7529299c2e6 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp @@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: DistanceBasedStaticMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex) + : VoxelGridStaticMapLoader(node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n"); } @@ -57,7 +57,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader DistanceBasedDynamicMapLoader( rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) - : VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group) + : VoxelGridDynamicMapLoader( + node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp index 3a5e183767620..56b09d2709cd9 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp @@ -34,7 +34,7 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader explicit VoxelBasedApproximateStaticMapLoader( rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) + : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n"); } @@ -49,7 +49,7 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) : VoxelGridDynamicMapLoader( - node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) + node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex, main_callback_group) { RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n"); } diff --git a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp index 0cfc8a64ab2dd..25ed245dba145 100644 --- a/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp +++ b/perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp @@ -42,9 +42,10 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader public: explicit VoxelDistanceBasedStaticMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex) - : VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) + : VoxelGridStaticMapLoader( + node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex) { RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n"); } @@ -60,11 +61,12 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader /* data */ public: explicit VoxelDistanceBasedDynamicMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double distance_threshold, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) : VoxelGridDynamicMapLoader( - node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group) + node, leaf_size, downsize_ratio_z_axis, distance_threshold, tf_map_input_frame, mutex, + main_callback_group) { RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n"); } 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 index 21cb19edcd0ec..c75005ffa56b8 100644 --- 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 @@ -97,6 +97,7 @@ class VoxelGridMapLoader double voxel_leaf_size_; double voxel_leaf_size_z_; double downsize_ratio_z_axis_; + double voxel_height_offset_; rclcpp::Publisher::SharedPtr downsampled_map_pub_; bool debug_ = false; @@ -105,7 +106,7 @@ class VoxelGridMapLoader typedef typename pcl::Filter::PointCloud PointCloud; typedef typename PointCloud::Ptr PointCloudPtr; explicit VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0; @@ -135,7 +136,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader public: explicit VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex); virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map); virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold); @@ -191,7 +192,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader public: explicit VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group); void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose); 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 82c3576dd951c..6dee1244d3ea2 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 @@ -46,16 +46,18 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); return; } + double voxel_height_offset = declare_parameter("voxel_height_offset"); set_map_in_voxel_grid_ = false; 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_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, - main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_, + &mutex_, main_callback_group); } else { voxel_grid_map_loader_ = std::make_unique( - this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_, + &mutex_); } tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_); RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str()); diff --git a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp index 32e5367fbcc38..9f8ce6fb16451 100644 --- a/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp +++ b/perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp @@ -113,6 +113,7 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC distance_threshold_ = declare_parameter("distance_threshold"); bool use_dynamic_map_loading = declare_parameter("use_dynamic_map_loading"); double downsize_ratio_z_axis = declare_parameter("downsize_ratio_z_axis"); + double voxel_height_offset = declare_parameter("voxel_height_offset"); if (downsize_ratio_z_axis <= 0.0) { RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive"); return; @@ -121,11 +122,12 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC rclcpp::CallbackGroup::SharedPtr main_callback_group; main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_, - main_callback_group); + this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_, + &mutex_, main_callback_group); } else { voxel_distance_based_map_loader_ = std::make_unique( - this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_); + this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_, + &mutex_); } } diff --git a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp index ef3727019c0f4..4d21c5ca6a5ea 100644 --- a/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp +++ b/perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp @@ -15,11 +15,12 @@ #include "compare_map_segmentation/voxel_grid_map_loader.hpp" VoxelGridMapLoader::VoxelGridMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex) : logger_(node->get_logger()), voxel_leaf_size_(leaf_size), - downsize_ratio_z_axis_(downsize_ratio_z_axis) + downsize_ratio_z_axis_(downsize_ratio_z_axis), + voxel_height_offset_(voxel_height_offset) { tf_map_input_frame_ = tf_map_input_frame; mutex_ptr_ = mutex; @@ -240,7 +241,7 @@ bool VoxelGridMapLoader::is_in_voxel( 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 dist_z = map->points.at(voxel_index).z - target_point.z - voxel_height_offset_; // check if the point is inside the distance threshold voxel if ( std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold && @@ -252,9 +253,10 @@ bool VoxelGridMapLoader::is_in_voxel( } VoxelGridStaticMapLoader::VoxelGridStaticMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex) -: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) +: VoxelGridMapLoader( + node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex) { voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; sub_map_ = node->create_subscription( @@ -292,10 +294,11 @@ bool VoxelGridStaticMapLoader::is_close_to_map( } VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader( - rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, + rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset, std::string * tf_map_input_frame, std::mutex * mutex, rclcpp::CallbackGroup::SharedPtr main_callback_group) -: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex) +: VoxelGridMapLoader( + node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex) { voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_; auto timer_interval_ms = node->declare_parameter("timer_interval_ms"); From 6c708aa58a33e6aad403800d4e1d2cd50015d804 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 16 Feb 2024 20:10:43 +0900 Subject: [PATCH 2/2] add parameter to yaml file Signed-off-by: yoshiri --- .../config/voxel_based_approximate_compare_map_filter.param.yaml | 1 + .../config/voxel_based_compare_map_filter.param.yaml | 1 + .../config/voxel_distance_based_compare_map_filter.param.yaml | 1 + 3 files changed, 3 insertions(+) diff --git a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml index d54f7a96ecda0..bcd41999402a2 100644 --- a/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml +++ b/perception/compare_map_segmentation/config/voxel_based_approximate_compare_map_filter.param.yaml @@ -3,6 +3,7 @@ distance_threshold: 0.5 use_dynamic_map_loading: true downsize_ratio_z_axis: 0.5 + voxel_height_offset: 0.0 timer_interval_ms: 100 map_update_distance_threshold: 10.0 map_loader_radius: 150.0 diff --git a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml index d54f7a96ecda0..bcd41999402a2 100644 --- a/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml +++ b/perception/compare_map_segmentation/config/voxel_based_compare_map_filter.param.yaml @@ -3,6 +3,7 @@ distance_threshold: 0.5 use_dynamic_map_loading: true downsize_ratio_z_axis: 0.5 + voxel_height_offset: 0.0 timer_interval_ms: 100 map_update_distance_threshold: 10.0 map_loader_radius: 150.0 diff --git a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml index d54f7a96ecda0..bcd41999402a2 100644 --- a/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml +++ b/perception/compare_map_segmentation/config/voxel_distance_based_compare_map_filter.param.yaml @@ -3,6 +3,7 @@ distance_threshold: 0.5 use_dynamic_map_loading: true downsize_ratio_z_axis: 0.5 + voxel_height_offset: 0.0 timer_interval_ms: 100 map_update_distance_threshold: 10.0 map_loader_radius: 150.0