From c708f98b41eb3b24cdb6bdae91b56ed657100909 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Thu, 27 Jun 2024 22:59:18 +0900 Subject: [PATCH] fix(euclidean_cluster): fix max_cluster_size bug Signed-off-by: badai-nguyen --- .../lib/voxel_grid_based_euclidean_cluster.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index 40699fd6e27ab..a12d1101ffe0b 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -117,7 +117,7 @@ bool VoxelGridBasedEuclideanCluster::cluster( voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); if (map.find(index) != map.end()) { auto & cluster_data_size = clusters_data_size.at(map[index]); - if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) { + if (cluster_data_size > std::size_t(max_cluster_size_ * point_step)) { continue; } std::memcpy(