13
13
// limitations under the License.
14
14
15
15
#include " autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
16
+
16
17
#include < rclcpp/node.hpp>
17
18
18
19
#include < pcl/kdtree/kdtree.h>
@@ -55,8 +56,9 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
55
56
56
57
std::stringstream summary;
57
58
if (!warnings.empty ()) {
58
- summary << warnings.size () << " clusters skipped because cluster size exceeds the maximum allowed "
59
- << max_cluster_size_ << " \n " ;
59
+ summary << warnings.size ()
60
+ << " clusters skipped because cluster size exceeds the maximum allowed "
61
+ << max_cluster_size_ << " \n " ;
60
62
for (const auto & warn : warnings) {
61
63
summary << " - " << warn << " \n " ;
62
64
}
@@ -65,12 +67,12 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
65
67
diagnostics_interface_ptr_->add_key_value (" is_cluster_data_size_within_range" , true );
66
68
}
67
69
diagnostics_interface_ptr_->update_level_and_message (
68
- warnings.empty () ? static_cast <int8_t >(diagnostic_msgs::msg::DiagnosticStatus::OK) : static_cast <int8_t >(diagnostic_msgs::msg::DiagnosticStatus::WARN),
70
+ warnings.empty () ? static_cast <int8_t >(diagnostic_msgs::msg::DiagnosticStatus::OK)
71
+ : static_cast <int8_t >(diagnostic_msgs::msg::DiagnosticStatus::WARN),
69
72
summary.str ());
70
73
diagnostics_interface_ptr_->publish (pointcloud_msg->header .stamp );
71
74
}
72
75
73
-
74
76
// TODO(badai-nguyen): remove this function when field copying also implemented for
75
77
// euclidean_cluster.cpp
76
78
bool VoxelGridBasedEuclideanCluster::cluster (
@@ -167,17 +169,18 @@ bool VoxelGridBasedEuclideanCluster::cluster(
167
169
// build output and check cluster size
168
170
{
169
171
// At the start, create a container to collect warnings.
170
- std::vector<std::string> warning_messages;
172
+ std::vector<std::string> warning_messages;
171
173
for (size_t i = 0 ; i < temporary_clusters.size (); ++i) {
172
174
auto & i_cluster_data_size = clusters_data_size.at (i);
173
175
int cluster_size = static_cast <int >(i_cluster_data_size / point_step);
174
176
if (cluster_size < min_cluster_size_) {
175
177
// Cluster size is below the minimum threshold; skip without messaging.
176
178
continue ;
177
179
}
178
- if (cluster_size > max_cluster_size_ ) {
180
+ if (cluster_size > max_cluster_size_) {
179
181
// Cluster size exceeds the maximum threshold; log a warning.
180
- warning_messages.push_back (" Cluster " + std::to_string (i) + " (" + std::to_string (cluster_size) + " )." );
182
+ warning_messages.push_back (
183
+ " Cluster " + std::to_string (i) + " (" + std::to_string (cluster_size) + " )." );
181
184
continue ;
182
185
}
183
186
const auto & cluster = temporary_clusters.at (i);
@@ -203,8 +206,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
203
206
}
204
207
objects.header = pointcloud_msg->header ;
205
208
206
- publishDiagnosticsSummary (warning_messages, pointcloud_msg);
207
-
209
+ publishDiagnosticsSummary (warning_messages, pointcloud_msg);
208
210
}
209
211
210
212
return true ;
0 commit comments