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..34c52bb7836b8 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -106,7 +106,7 @@ bool VoxelGridBasedEuclideanCluster::cluster( temporary_cluster.height = pointcloud_msg->height; temporary_cluster.fields = pointcloud_msg->fields; temporary_cluster.point_step = point_step; - temporary_cluster.data.resize(max_cluster_size_ * point_step); + temporary_cluster.data.resize(cluster.indices.size() * point_step); clusters_data_size.push_back(0); } @@ -124,6 +124,10 @@ bool VoxelGridBasedEuclideanCluster::cluster( &temporary_clusters.at(map[index]).data[cluster_data_size], &pointcloud_msg->data[i * point_step], point_step); cluster_data_size += point_step; + if (cluster_data_size == temporary_clusters.at(map[index]).data.size()) { + temporary_clusters.at(map[index]) + .data.resize(temporary_clusters.at(map[index]).data.size() * 2); + } } }