From aa613d47c81b270f149b853f4fb4ca511f7a954f Mon Sep 17 00:00:00 2001
From: "lei.gu" <lei.gu@tier4.jp>
Date: Mon, 10 Mar 2025 13:22:55 +0900
Subject: [PATCH 1/5] feat(euclidean_cluster): add diagnostics warning when
cluster skipped due to excessive points from large objects
---
.../voxel_grid_based_euclidean_cluster.hpp | 13 ++++-
.../voxel_grid_based_euclidean_cluster.cpp | 50 ++++++++++++++++++-
...oxel_grid_based_euclidean_cluster_node.cpp | 8 ++-
...oxel_grid_based_euclidean_cluster_node.hpp | 5 +-
4 files changed, 70 insertions(+), 6 deletions(-)
diff --git a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
index 5c51bb85ce177..a66478bd1c4fc 100644
--- a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
+++ b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
@@ -13,9 +13,10 @@
// limitations under the License.
#pragma once
-
#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp"
#include "autoware/euclidean_cluster/utils.hpp"
+#include <autoware_utils/ros/diagnostics_interface.hpp>
+#include <rclcpp/node.hpp>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
@@ -44,12 +45,20 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
{
min_points_number_per_voxel_ = min_points_number_per_voxel;
}
-
+ void setDiagnosticsInterface(autoware_utils::DiagnosticsInterface* diag_ptr)
+ {
+ diagnostics_interface_ptr_ = diag_ptr;
+ }
private:
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
float tolerance_;
float voxel_leaf_size_;
int min_points_number_per_voxel_;
+
+ void publishDiagnosticsSummary(
+ const std::vector<std::string> & warnings,
+ const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg);
+ autoware_utils::DiagnosticsInterface* diagnostics_interface_ptr_{nullptr};
};
} // namespace autoware::euclidean_cluster
diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
index 5f4ef98384d23..265415996d35a 100644
--- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
+++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -13,6 +13,7 @@
// limitations under the License.
#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
+#include <rclcpp/node.hpp>
#include <pcl/kdtree/kdtree.h>
#include <pcl/segmentation/extract_clusters.h>
@@ -41,6 +42,35 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
min_points_number_per_voxel_(min_points_number_per_voxel)
{
}
+
+// After processing all clusters, publish a summary of diagnostics.
+void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
+ const std::vector<std::string> & warnings,
+ const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg)
+{
+ if (!diagnostics_interface_ptr_) {
+ return;
+ }
+ diagnostics_interface_ptr_->clear();
+
+ std::stringstream summary;
+ if (!warnings.empty()) {
+ summary << warnings.size() << " clusters skipped because cluster size exceeds the maximum allowed "
+ << max_cluster_size_ << " \n";
+ for (const auto & warn : warnings) {
+ summary << " - " << warn << "\n";
+ }
+ diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", false);
+ } else {
+ diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
+ }
+ diagnostics_interface_ptr_->update_level_and_message(
+ warnings.empty() ? diagnostic_msgs::msg::DiagnosticStatus::OK : diagnostic_msgs::msg::DiagnosticStatus::WARN,
+ summary.str());
+ diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
+}
+
+
// TODO(badai-nguyen): remove this function when field copying also implemented for
// euclidean_cluster.cpp
bool VoxelGridBasedEuclideanCluster::cluster(
@@ -136,12 +166,25 @@ bool VoxelGridBasedEuclideanCluster::cluster(
// build output and check cluster size
{
+ // At the start, create a container to collect warnings.
+ std::vector<std::string> warning_messages;
for (size_t i = 0; i < temporary_clusters.size(); ++i) {
auto & i_cluster_data_size = clusters_data_size.at(i);
- if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
- static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
+ int cluster_size = static_cast<int>(i_cluster_data_size / point_step);
+ if (cluster_size < min_cluster_size_) {
+ // Cluster size is below the minimum threshold; skip without messaging.
+ continue;
+ }
+ if (cluster_size > max_cluster_size_ /10) {
+ // Cluster size exceeds the maximum threshold; log a warning.
+ warning_messages.push_back(" Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
continue;
}
+
+ // if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
+ // static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
+ // continue;
+ // }
const auto & cluster = temporary_clusters.at(i);
tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
feature_object.feature.cluster = cluster;
@@ -164,6 +207,9 @@ bool VoxelGridBasedEuclideanCluster::cluster(
objects.feature_objects.push_back(feature_object);
}
objects.header = pointcloud_msg->header;
+
+ publishDiagnosticsSummary(warning_messages, pointcloud_msg);
+
}
return true;
diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
index 1f60e18fe3c81..32d65f197e6c7 100644
--- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
+++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
@@ -31,10 +31,14 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
const float tolerance = this->declare_parameter("tolerance", 1.0);
const float voxel_leaf_size = this->declare_parameter("voxel_leaf_size", 0.5);
const int min_points_number_per_voxel = this->declare_parameter("min_points_number_per_voxel", 3);
+
cluster_ = std::make_shared<VoxelGridBasedEuclideanCluster>(
use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size,
min_points_number_per_voxel);
-
+ // Pass the diagnostics interface pointer from the node to the cluster
+ diagnostics_interface_ptr_ = std::make_unique<autoware_utils::DiagnosticsInterface>(this, "euclidean_cluster");
+ cluster_->setDiagnosticsInterface(diagnostics_interface_ptr_.get());
+
using std::placeholders::_1;
pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"input", rclcpp::SensorDataQoS().keep_last(1),
@@ -48,6 +52,8 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
std::make_unique<autoware_utils::DebugPublisher>(this, "voxel_grid_based_euclidean_cluster");
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
+
+
}
void VoxelGridBasedEuclideanClusterNode::onPointCloud(
diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
index 8ac8e7aef84dc..ca407141d0457 100644
--- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
+++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
@@ -18,7 +18,7 @@
#include <autoware_utils/ros/debug_publisher.hpp>
#include <autoware_utils/system/stop_watch.hpp>
-#include <rclcpp/rclcpp.hpp>
+#include <autoware_utils/ros/diagnostics_interface.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
@@ -43,6 +43,9 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node
std::shared_ptr<VoxelGridBasedEuclideanCluster> cluster_;
std::unique_ptr<autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
+
+ std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
+
};
} // namespace autoware::euclidean_cluster
From a5ef9bb5d790f019a369e4364fe842548024b3d3 Mon Sep 17 00:00:00 2001
From: "lei.gu" <lei.gu@tier4.jp>
Date: Mon, 17 Mar 2025 10:07:06 +0900
Subject: [PATCH 2/5] remove temporary code
---
.../lib/voxel_grid_based_euclidean_cluster.cpp | 9 ++-------
1 file changed, 2 insertions(+), 7 deletions(-)
diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
index 265415996d35a..cf5dadb177031 100644
--- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
+++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -65,7 +65,7 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
}
diagnostics_interface_ptr_->update_level_and_message(
- warnings.empty() ? diagnostic_msgs::msg::DiagnosticStatus::OK : diagnostic_msgs::msg::DiagnosticStatus::WARN,
+ warnings.empty() ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK) : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN),
summary.str());
diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
}
@@ -175,16 +175,11 @@ bool VoxelGridBasedEuclideanCluster::cluster(
// Cluster size is below the minimum threshold; skip without messaging.
continue;
}
- if (cluster_size > max_cluster_size_ /10) {
+ if (cluster_size > max_cluster_size_ ) {
// Cluster size exceeds the maximum threshold; log a warning.
warning_messages.push_back(" Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
continue;
}
-
- // if (!(min_cluster_size_ <= static_cast<int>(i_cluster_data_size / point_step) &&
- // static_cast<int>(i_cluster_data_size / point_step) <= max_cluster_size_)) {
- // continue;
- // }
const auto & cluster = temporary_clusters.at(i);
tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object;
feature_object.feature.cluster = cluster;
From ac9620a64703ee50468aed3e63c1080576caae02 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Mon, 17 Mar 2025 05:47:30 +0000
Subject: [PATCH 3/5] style(pre-commit): autofix
---
.../voxel_grid_based_euclidean_cluster.hpp | 6 ++++--
.../voxel_grid_based_euclidean_cluster.cpp | 20 ++++++++++---------
...oxel_grid_based_euclidean_cluster_node.cpp | 7 +++----
...oxel_grid_based_euclidean_cluster_node.hpp | 3 +--
4 files changed, 19 insertions(+), 17 deletions(-)
diff --git a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
index a66478bd1c4fc..16a62f8bbfc84 100644
--- a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
+++ b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
@@ -15,6 +15,7 @@
#pragma once
#include "autoware/euclidean_cluster/euclidean_cluster_interface.hpp"
#include "autoware/euclidean_cluster/utils.hpp"
+
#include <autoware_utils/ros/diagnostics_interface.hpp>
#include <rclcpp/node.hpp>
@@ -45,10 +46,11 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
{
min_points_number_per_voxel_ = min_points_number_per_voxel;
}
- void setDiagnosticsInterface(autoware_utils::DiagnosticsInterface* diag_ptr)
+ void setDiagnosticsInterface(autoware_utils::DiagnosticsInterface * diag_ptr)
{
diagnostics_interface_ptr_ = diag_ptr;
}
+
private:
pcl::VoxelGrid<pcl::PointXYZ> voxel_grid_;
float tolerance_;
@@ -58,7 +60,7 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
void publishDiagnosticsSummary(
const std::vector<std::string> & warnings,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg);
- autoware_utils::DiagnosticsInterface* diagnostics_interface_ptr_{nullptr};
+ autoware_utils::DiagnosticsInterface * diagnostics_interface_ptr_{nullptr};
};
} // namespace autoware::euclidean_cluster
diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
index cf5dadb177031..33a63886d2f49 100644
--- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
+++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -13,6 +13,7 @@
// limitations under the License.
#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
+
#include <rclcpp/node.hpp>
#include <pcl/kdtree/kdtree.h>
@@ -55,8 +56,9 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
std::stringstream summary;
if (!warnings.empty()) {
- summary << warnings.size() << " clusters skipped because cluster size exceeds the maximum allowed "
- << max_cluster_size_ << " \n";
+ summary << warnings.size()
+ << " clusters skipped because cluster size exceeds the maximum allowed "
+ << max_cluster_size_ << " \n";
for (const auto & warn : warnings) {
summary << " - " << warn << "\n";
}
@@ -65,12 +67,12 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
}
diagnostics_interface_ptr_->update_level_and_message(
- warnings.empty() ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK) : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN),
+ warnings.empty() ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK)
+ : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN),
summary.str());
diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
}
-
// TODO(badai-nguyen): remove this function when field copying also implemented for
// euclidean_cluster.cpp
bool VoxelGridBasedEuclideanCluster::cluster(
@@ -167,7 +169,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
// build output and check cluster size
{
// At the start, create a container to collect warnings.
- std::vector<std::string> warning_messages;
+ std::vector<std::string> warning_messages;
for (size_t i = 0; i < temporary_clusters.size(); ++i) {
auto & i_cluster_data_size = clusters_data_size.at(i);
int cluster_size = static_cast<int>(i_cluster_data_size / point_step);
@@ -175,9 +177,10 @@ bool VoxelGridBasedEuclideanCluster::cluster(
// Cluster size is below the minimum threshold; skip without messaging.
continue;
}
- if (cluster_size > max_cluster_size_ ) {
+ if (cluster_size > max_cluster_size_) {
// Cluster size exceeds the maximum threshold; log a warning.
- warning_messages.push_back(" Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
+ warning_messages.push_back(
+ " Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
continue;
}
const auto & cluster = temporary_clusters.at(i);
@@ -203,8 +206,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
}
objects.header = pointcloud_msg->header;
- publishDiagnosticsSummary(warning_messages, pointcloud_msg);
-
+ publishDiagnosticsSummary(warning_messages, pointcloud_msg);
}
return true;
diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
index 32d65f197e6c7..82e7cf5b7f803 100644
--- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
+++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp
@@ -36,9 +36,10 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
use_height, min_cluster_size, max_cluster_size, tolerance, voxel_leaf_size,
min_points_number_per_voxel);
// Pass the diagnostics interface pointer from the node to the cluster
- diagnostics_interface_ptr_ = std::make_unique<autoware_utils::DiagnosticsInterface>(this, "euclidean_cluster");
+ diagnostics_interface_ptr_ =
+ std::make_unique<autoware_utils::DiagnosticsInterface>(this, "euclidean_cluster");
cluster_->setDiagnosticsInterface(diagnostics_interface_ptr_.get());
-
+
using std::placeholders::_1;
pointcloud_sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"input", rclcpp::SensorDataQoS().keep_last(1),
@@ -52,8 +53,6 @@ VoxelGridBasedEuclideanClusterNode::VoxelGridBasedEuclideanClusterNode(
std::make_unique<autoware_utils::DebugPublisher>(this, "voxel_grid_based_euclidean_cluster");
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
-
-
}
void VoxelGridBasedEuclideanClusterNode::onPointCloud(
diff --git a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
index ca407141d0457..db65ffdf224c3 100644
--- a/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
+++ b/perception/autoware_euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.hpp
@@ -17,8 +17,8 @@
#include "autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp"
#include <autoware_utils/ros/debug_publisher.hpp>
-#include <autoware_utils/system/stop_watch.hpp>
#include <autoware_utils/ros/diagnostics_interface.hpp>
+#include <autoware_utils/system/stop_watch.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
@@ -45,7 +45,6 @@ class VoxelGridBasedEuclideanClusterNode : public rclcpp::Node
std::unique_ptr<autoware_utils::DebugPublisher> debug_publisher_;
std::unique_ptr<autoware_utils::DiagnosticsInterface> diagnostics_interface_ptr_;
-
};
} // namespace autoware::euclidean_cluster
From 773996066e36a8b27a8fc09c98132718f19933c0 Mon Sep 17 00:00:00 2001
From: "lei.gu" <lei.gu@tier4.jp>
Date: Wed, 19 Mar 2025 11:56:23 +0900
Subject: [PATCH 4/5] feat(euclidean_cluster): diagnostics modified to remove
redundant info
---
.../voxel_grid_based_euclidean_cluster.hpp | 2 +-
.../voxel_grid_based_euclidean_cluster.cpp | 30 +++++++------------
2 files changed, 12 insertions(+), 20 deletions(-)
diff --git a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
index 16a62f8bbfc84..12f93d2f4cbcb 100644
--- a/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
+++ b/perception/autoware_euclidean_cluster/include/autoware/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp
@@ -58,7 +58,7 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface
int min_points_number_per_voxel_;
void publishDiagnosticsSummary(
- const std::vector<std::string> & warnings,
+ size_t skipped_cluster_count,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg);
autoware_utils::DiagnosticsInterface * diagnostics_interface_ptr_{nullptr};
};
diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
index 33a63886d2f49..45597e37001d3 100644
--- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
+++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -46,30 +46,24 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster(
// After processing all clusters, publish a summary of diagnostics.
void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
- const std::vector<std::string> & warnings,
+ size_t skipped_cluster_count,
const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg)
{
if (!diagnostics_interface_ptr_) {
return;
}
diagnostics_interface_ptr_->clear();
-
- std::stringstream summary;
- if (!warnings.empty()) {
- summary << warnings.size()
- << " clusters skipped because cluster size exceeds the maximum allowed "
- << max_cluster_size_ << " \n";
- for (const auto & warn : warnings) {
- summary << " - " << warn << "\n";
- }
+ std::string summary;
+ if (skipped_cluster_count > 0) {
+ summary = std::to_string(skipped_cluster_count) + " clusters skipped because cluster point size exceeds the maximum allowed "
+ + std::to_string(max_cluster_size_);
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", false);
} else {
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
}
diagnostics_interface_ptr_->update_level_and_message(
- warnings.empty() ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK)
- : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN),
- summary.str());
+ skipped_cluster_count > 0 ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN) : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK),
+ summary);
diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
}
@@ -168,8 +162,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
// build output and check cluster size
{
- // At the start, create a container to collect warnings.
- std::vector<std::string> warning_messages;
+ size_t skipped_cluster_count = 0; // Count the skipped clusters
for (size_t i = 0; i < temporary_clusters.size(); ++i) {
auto & i_cluster_data_size = clusters_data_size.at(i);
int cluster_size = static_cast<int>(i_cluster_data_size / point_step);
@@ -179,8 +172,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
}
if (cluster_size > max_cluster_size_) {
// Cluster size exceeds the maximum threshold; log a warning.
- warning_messages.push_back(
- " Cluster " + std::to_string(i) + " (" + std::to_string(cluster_size) + ").");
+ skipped_cluster_count++;
continue;
}
const auto & cluster = temporary_clusters.at(i);
@@ -205,8 +197,8 @@ bool VoxelGridBasedEuclideanCluster::cluster(
objects.feature_objects.push_back(feature_object);
}
objects.header = pointcloud_msg->header;
-
- publishDiagnosticsSummary(warning_messages, pointcloud_msg);
+ // Publish the diagnostics summary.
+ publishDiagnosticsSummary(skipped_cluster_count, pointcloud_msg);
}
return true;
From 9f70a47d217155aaad657e116e78b9549d2de03e Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
<66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Wed, 19 Mar 2025 03:06:54 +0000
Subject: [PATCH 5/5] style(pre-commit): autofix
---
.../lib/voxel_grid_based_euclidean_cluster.cpp | 10 ++++++----
1 file changed, 6 insertions(+), 4 deletions(-)
diff --git a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
index 45597e37001d3..34e1cc16155cd 100644
--- a/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
+++ b/perception/autoware_euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp
@@ -55,14 +55,16 @@ void VoxelGridBasedEuclideanCluster::publishDiagnosticsSummary(
diagnostics_interface_ptr_->clear();
std::string summary;
if (skipped_cluster_count > 0) {
- summary = std::to_string(skipped_cluster_count) + " clusters skipped because cluster point size exceeds the maximum allowed "
- + std::to_string(max_cluster_size_);
+ summary = std::to_string(skipped_cluster_count) +
+ " clusters skipped because cluster point size exceeds the maximum allowed " +
+ std::to_string(max_cluster_size_);
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", false);
} else {
diagnostics_interface_ptr_->add_key_value("is_cluster_data_size_within_range", true);
}
diagnostics_interface_ptr_->update_level_and_message(
- skipped_cluster_count > 0 ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN) : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK),
+ skipped_cluster_count > 0 ? static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::WARN)
+ : static_cast<int8_t>(diagnostic_msgs::msg::DiagnosticStatus::OK),
summary);
diagnostics_interface_ptr_->publish(pointcloud_msg->header.stamp);
}
@@ -198,7 +200,7 @@ bool VoxelGridBasedEuclideanCluster::cluster(
}
objects.header = pointcloud_msg->header;
// Publish the diagnostics summary.
- publishDiagnosticsSummary(skipped_cluster_count, pointcloud_msg);
+ publishDiagnosticsSummary(skipped_cluster_count, pointcloud_msg);
}
return true;