Skip to content

Commit e0de9bb

Browse files
committed
feat: add voxel height offset parameter
Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
1 parent 7938920 commit e0de9bb

7 files changed

+35
-24
lines changed

perception/compare_map_segmentation/include/compare_map_segmentation/distance_based_compare_map_filter_nodelet.hpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -42,7 +42,7 @@ class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
4242
public:
4343
DistanceBasedStaticMapLoader(
4444
rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex)
45-
: VoxelGridStaticMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex)
45+
: VoxelGridStaticMapLoader(node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex)
4646
{
4747
RCLCPP_INFO(logger_, "DistanceBasedStaticMapLoader initialized.\n");
4848
}
@@ -57,7 +57,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
5757
DistanceBasedDynamicMapLoader(
5858
rclcpp::Node * node, double leaf_size, std::string * tf_map_input_frame, std::mutex * mutex,
5959
rclcpp::CallbackGroup::SharedPtr main_callback_group)
60-
: VoxelGridDynamicMapLoader(node, leaf_size, 1.0, tf_map_input_frame, mutex, main_callback_group)
60+
: VoxelGridDynamicMapLoader(
61+
node, leaf_size, 1.0, 0.0, tf_map_input_frame, mutex, main_callback_group)
6162
{
6263
RCLCPP_INFO(logger_, "DistanceBasedDynamicMapLoader initialized.\n");
6364
}

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_based_approximate_compare_map_filter_nodelet.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,7 @@ class VoxelBasedApproximateStaticMapLoader : public VoxelGridStaticMapLoader
3434
explicit VoxelBasedApproximateStaticMapLoader(
3535
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
3636
std::string * tf_map_input_frame, std::mutex * mutex)
37-
: VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
37+
: VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex)
3838
{
3939
RCLCPP_INFO(logger_, "VoxelBasedApproximateStaticMapLoader initialized.\n");
4040
}
@@ -49,7 +49,7 @@ class VoxelBasedApproximateDynamicMapLoader : public VoxelGridDynamicMapLoader
4949
std::string * tf_map_input_frame, std::mutex * mutex,
5050
rclcpp::CallbackGroup::SharedPtr main_callback_group)
5151
: VoxelGridDynamicMapLoader(
52-
node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
52+
node, leaf_size, downsize_ratio_z_axis, 0.0, tf_map_input_frame, mutex, main_callback_group)
5353
{
5454
RCLCPP_INFO(logger_, "VoxelBasedApproximateDynamicMapLoader initialized.\n");
5555
}

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_distance_based_compare_map_filter_nodelet.hpp

+6-4
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,10 @@ class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
4242

4343
public:
4444
explicit VoxelDistanceBasedStaticMapLoader(
45-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
45+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
4646
std::string * tf_map_input_frame, std::mutex * mutex)
47-
: VoxelGridStaticMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
47+
: VoxelGridStaticMapLoader(
48+
node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
4849
{
4950
RCLCPP_INFO(logger_, "VoxelDistanceBasedStaticMapLoader initialized.\n");
5051
}
@@ -60,11 +61,12 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
6061
/* data */
6162
public:
6263
explicit VoxelDistanceBasedDynamicMapLoader(
63-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
64+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double distance_threshold,
6465
std::string * tf_map_input_frame, std::mutex * mutex,
6566
rclcpp::CallbackGroup::SharedPtr main_callback_group)
6667
: VoxelGridDynamicMapLoader(
67-
node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex, main_callback_group)
68+
node, leaf_size, downsize_ratio_z_axis, distance_threshold, tf_map_input_frame, mutex,
69+
main_callback_group)
6870
{
6971
RCLCPP_INFO(logger_, "VoxelDistanceBasedDynamicMapLoader initialized.\n");
7072
}

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

+4-3
Original file line numberDiff line numberDiff line change
@@ -97,6 +97,7 @@ class VoxelGridMapLoader
9797
double voxel_leaf_size_;
9898
double voxel_leaf_size_z_;
9999
double downsize_ratio_z_axis_;
100+
double voxel_height_offset_;
100101
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr downsampled_map_pub_;
101102
bool debug_ = false;
102103

@@ -105,7 +106,7 @@ class VoxelGridMapLoader
105106
typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
106107
typedef typename PointCloud::Ptr PointCloudPtr;
107108
explicit VoxelGridMapLoader(
108-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
109+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
109110
std::string * tf_map_input_frame, std::mutex * mutex);
110111

111112
virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
@@ -135,7 +136,7 @@ class VoxelGridStaticMapLoader : public VoxelGridMapLoader
135136

136137
public:
137138
explicit VoxelGridStaticMapLoader(
138-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
139+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
139140
std::string * tf_map_input_frame, std::mutex * mutex);
140141
virtual void onMapCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr map);
141142
virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold);
@@ -191,7 +192,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
191192

192193
public:
193194
explicit VoxelGridDynamicMapLoader(
194-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
195+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
195196
std::string * tf_map_input_frame, std::mutex * mutex,
196197
rclcpp::CallbackGroup::SharedPtr main_callback_group);
197198
void onEstimatedPoseCallback(nav_msgs::msg::Odometry::ConstSharedPtr pose);

perception/compare_map_segmentation/src/voxel_based_compare_map_filter_nodelet.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -46,16 +46,18 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
4646
RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
4747
return;
4848
}
49+
double voxel_height_offset = declare_parameter<double>("voxel_height_offset");
4950
set_map_in_voxel_grid_ = false;
5051
if (use_dynamic_map_loading) {
5152
rclcpp::CallbackGroup::SharedPtr main_callback_group;
5253
main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
5354
voxel_grid_map_loader_ = std::make_unique<VoxelGridDynamicMapLoader>(
54-
this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
55-
main_callback_group);
55+
this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
56+
&mutex_, main_callback_group);
5657
} else {
5758
voxel_grid_map_loader_ = std::make_unique<VoxelGridStaticMapLoader>(
58-
this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
59+
this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
60+
&mutex_);
5961
}
6062
tf_input_frame_ = *(voxel_grid_map_loader_->tf_map_input_frame_);
6163
RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());

perception/compare_map_segmentation/src/voxel_distance_based_compare_map_filter_nodelet.cpp

+5-3
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,7 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC
113113
distance_threshold_ = declare_parameter<double>("distance_threshold");
114114
bool use_dynamic_map_loading = declare_parameter<bool>("use_dynamic_map_loading");
115115
double downsize_ratio_z_axis = declare_parameter<double>("downsize_ratio_z_axis");
116+
double voxel_height_offset = declare_parameter<double>("voxel_height_offset");
116117
if (downsize_ratio_z_axis <= 0.0) {
117118
RCLCPP_ERROR(this->get_logger(), "downsize_ratio_z_axis should be positive");
118119
return;
@@ -121,11 +122,12 @@ VoxelDistanceBasedCompareMapFilterComponent::VoxelDistanceBasedCompareMapFilterC
121122
rclcpp::CallbackGroup::SharedPtr main_callback_group;
122123
main_callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
123124
voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedDynamicMapLoader>(
124-
this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_,
125-
main_callback_group);
125+
this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
126+
&mutex_, main_callback_group);
126127
} else {
127128
voxel_distance_based_map_loader_ = std::make_unique<VoxelDistanceBasedStaticMapLoader>(
128-
this, distance_threshold_, downsize_ratio_z_axis, &tf_input_frame_, &mutex_);
129+
this, distance_threshold_, downsize_ratio_z_axis, voxel_height_offset, &tf_input_frame_,
130+
&mutex_);
129131
}
130132
}
131133

perception/compare_map_segmentation/src/voxel_grid_map_loader.cpp

+10-7
Original file line numberDiff line numberDiff line change
@@ -15,11 +15,12 @@
1515
#include "compare_map_segmentation/voxel_grid_map_loader.hpp"
1616

1717
VoxelGridMapLoader::VoxelGridMapLoader(
18-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
18+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
1919
std::string * tf_map_input_frame, std::mutex * mutex)
2020
: logger_(node->get_logger()),
2121
voxel_leaf_size_(leaf_size),
22-
downsize_ratio_z_axis_(downsize_ratio_z_axis)
22+
downsize_ratio_z_axis_(downsize_ratio_z_axis),
23+
voxel_height_offset_(voxel_height_offset)
2324
{
2425
tf_map_input_frame_ = tf_map_input_frame;
2526
mutex_ptr_ = mutex;
@@ -240,7 +241,7 @@ bool VoxelGridMapLoader::is_in_voxel(
240241
if (voxel_index != -1) { // not empty voxel
241242
const double dist_x = map->points.at(voxel_index).x - target_point.x;
242243
const double dist_y = map->points.at(voxel_index).y - target_point.y;
243-
const double dist_z = map->points.at(voxel_index).z - target_point.z;
244+
const double dist_z = map->points.at(voxel_index).z - target_point.z - voxel_height_offset_;
244245
// check if the point is inside the distance threshold voxel
245246
if (
246247
std::abs(dist_x) < distance_threshold && std::abs(dist_y) < distance_threshold &&
@@ -252,9 +253,10 @@ bool VoxelGridMapLoader::is_in_voxel(
252253
}
253254

254255
VoxelGridStaticMapLoader::VoxelGridStaticMapLoader(
255-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
256+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
256257
std::string * tf_map_input_frame, std::mutex * mutex)
257-
: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
258+
: VoxelGridMapLoader(
259+
node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
258260
{
259261
voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
260262
sub_map_ = node->create_subscription<sensor_msgs::msg::PointCloud2>(
@@ -292,10 +294,11 @@ bool VoxelGridStaticMapLoader::is_close_to_map(
292294
}
293295

294296
VoxelGridDynamicMapLoader::VoxelGridDynamicMapLoader(
295-
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
297+
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis, double voxel_height_offset,
296298
std::string * tf_map_input_frame, std::mutex * mutex,
297299
rclcpp::CallbackGroup::SharedPtr main_callback_group)
298-
: VoxelGridMapLoader(node, leaf_size, downsize_ratio_z_axis, tf_map_input_frame, mutex)
300+
: VoxelGridMapLoader(
301+
node, leaf_size, downsize_ratio_z_axis, voxel_height_offset, tf_map_input_frame, mutex)
299302
{
300303
voxel_leaf_size_z_ = voxel_leaf_size_ * downsize_ratio_z_axis_;
301304
auto timer_interval_ms = node->declare_parameter<int>("timer_interval_ms");

0 commit comments

Comments
 (0)