#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <chrono>
#include <iostream>
#include <vector>
#include <cmath>
#include <iomanip>
#include <sstream>
#include <memory>
#include <queue>
#include <unordered_set>
// 边界框结构体
struct Box {
float x_min;
float y_min;
float z_min;
float x_max;
float y_max;
float z_max;
};
// 锥桶信息结构体
struct Cone {
pcl::PointXYZ center;
float height;
float width;
float confidence;
int id;
std::string type;
};
// 自定义点云有效性检查函数
template<typename PointT>
bool isPointFinite(const PointT& point) {
return std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z);
}
// 点云处理模板类
template<typename PointT>
class ProcessPointClouds {
public:
// 自定义欧式距离聚类算法
std::vector<typename pcl::PointCloud<PointT>::Ptr>
EuclideanClustering(typename pcl::PointCloud<PointT>::Ptr cloud,
float clusterTolerance, int minSize, int maxSize) {
auto startTime = std::chrono::steady_clock::now();
std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;
std::cout << "=== 欧式距离聚类开始 ===" << std::endl;
std::cout << "输入点云数量: " << cloud->size() << std::endl;
std::cout << "聚类参数: 距离容忍度=" << clusterTolerance
<< ", 最小尺寸=" << minSize
<< ", 最大尺寸=" << maxSize << std::endl;
// 检查点云是否为空
if (cloud->empty()) {
std::cout << "点云为空,无法进行聚类" << std::endl;
return clusters;
}
// 创建KdTree用于加速近邻搜索
typename pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
try {
auto filtered_cloud = RemoveNaNPoints(cloud);
tree->setInputCloud(filtered_cloud);
std::cout << "KdTree构建成功" << std::endl;
} catch (const std::exception& e) {
std::cout << "KdTree构建失败: " << e.what() << std::endl;
return clusters;
}
// 标记已访问的点
std::vector<bool> processed(cloud->points.size(), false);
int total_clusters_found = 0;
int skipped_clusters = 0;
// 对每个点进行聚类
for (size_t i = 0; i < cloud->points.size(); ++i) {
// 跳过已处理或无效的点
if (processed[i] || !isPointFinite(cloud->points[i])) {
continue;
}
// 创建新的聚类
std::vector<int> cluster_indices;
std::queue<int> points_queue;
// 将当前点加入队列
points_queue.push(i);
processed[i] = true;
// 区域生长聚类
while (!points_queue.empty()) {
// 获取队列中的点
int current_index = points_queue.front();
points_queue.pop();
cluster_indices.push_back(current_index);
// 搜索近邻点
std::vector<int> neighbor_indices;
std::vector<float> neighbor_distances;
if (tree->radiusSearch(cloud->points[current_index],
clusterTolerance,
neighbor_indices,
neighbor_distances) > 0) {
for (const auto& neighbor_index : neighbor_indices) {
if (!processed[neighbor_index] && isPointFinite(cloud->points[neighbor_index])) {
processed[neighbor_index] = true;
points_queue.push(neighbor_index);
}
}
}
}
// 检查聚类尺寸是否在范围内
size_t cluster_size = cluster_indices.size();
total_clusters_found++;
if (cluster_size >= static_cast<size_t>(minSize) && cluster_size <= static_cast<size_t>(maxSize)) {
// 创建聚类点云
typename pcl::PointCloud<PointT>::Ptr cluster_cloud(new pcl::PointCloud<PointT>);
for (const auto& index : cluster_indices) {
cluster_cloud->points.push_back(cloud->points[index]);
}
cluster_cloud->width = cluster_cloud->points.size();
cluster_cloud->height = 1;
cluster_cloud->is_dense = true;
clusters.push_back(cluster_cloud);
std::cout << "聚类 " << clusters.size() << ": " << cluster_cloud->size() << " 个点" << std::endl;
} else {
skipped_clusters++;
if (cluster_size < static_cast<size_t>(minSize)) {
std::cout << "跳过小聚类: " << cluster_size << " 个点 (小于 " << minSize << ")" << std::endl;
} else {
std::cout << "跳过大聚类: " << cluster_size << " 个点 (大于 " << maxSize << ")" << std::endl;
}
}
}
auto endTime = std::chrono::steady_clock::now();
auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime);
std::cout << "欧式距离聚类完成,耗时 " << elapsedTime.count()
<< " 毫秒,找到 " << clusters.size() << " 个有效聚类 ("
<< skipped_clusters << " 个被跳过,总共 " << total_clusters_found << " 个聚类)" << std::endl;
return clusters;
}
// 计算两个点之间的欧式距离
float EuclideanDistance(const PointT& point1, const PointT& point2) {
float dx = point1.x - point2.x;
float dy = point1.y - point2.y;
float dz = point1.z - point2.z;
return std::sqrt(dx*dx + dy*dy + dz*dz);
}
// 计算边界框
Box BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster) {
PointT minPoint, maxPoint;
pcl::getMinMax3D(*cluster, minPoint, maxPoint);
Box box;
box.x_min = minPoint.x;
box.y_min = minPoint.y;
box.z_min = minPoint.z;
box.x_max = maxPoint.x;
box.y_max = maxPoint.y;
box.z_max = maxPoint.z;
return box;
}
// 过滤无效点
typename pcl::PointCloud<PointT>::Ptr RemoveNaNPoints(typename pcl::PointCloud<PointT>::Ptr cloud) {
typename pcl::PointCloud<PointT>::Ptr filtered_cloud(new pcl::PointCloud<PointT>);
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*cloud, *filtered_cloud, indices);
std::cout << "过滤掉 " << (cloud->size() - filtered_cloud->size())
<< " 个无效点,剩余 " << filtered_cloud->size() << " 个有效点" << std::endl;
return filtered_cloud;
}
// 降采样点云
typename pcl::PointCloud<PointT>::Ptr DownsampleCloud(typename pcl::PointCloud<PointT>::Ptr cloud, float leaf_size) {
typename pcl::PointCloud<PointT>::Ptr downsampled_cloud(new pcl::PointCloud<PointT>);
pcl::VoxelGrid<PointT> voxel_grid;
voxel_grid.setInputCloud(cloud);
voxel_grid.setLeafSize(leaf_size, leaf_size, leaf_size);
voxel_grid.filter(*downsampled_cloud);
std::cout << "降采样后点云: " << cloud->size() << " -> " << downsampled_cloud->size() << " 个点" << std::endl;
return downsampled_cloud;
}
// 直通滤波
typename pcl::PointCloud<PointT>::Ptr PassThroughFilter(typename pcl::PointCloud<PointT>::Ptr cloud,
const std::string& field,
float min_limit, float max_limit) {
typename pcl::PointCloud<PointT>::Ptr filtered_cloud(new pcl::PointCloud<PointT>);
pcl::PassThrough<PointT> pass;
pass.setInputCloud(cloud);
pass.setFilterFieldName(field);
pass.setFilterLimits(min_limit, max_limit);
pass.filter(*filtered_cloud);
std::cout << "直通滤波(" << field << "): " << cloud->size() << " -> " << filtered_cloud->size()
<< " 个点, 范围[" << min_limit << ", " << max_limit << "]" << std::endl;
return filtered_cloud;
}
// 锥桶识别函数
std::vector<Cone> DetectCones(typename pcl::PointCloud<PointT>::Ptr cloud,
float min_height = 0.05, float max_height = 0.5,
float min_width = 0.05, float max_width = 0.25,
float height_to_width_ratio_min = 0.8,
float height_to_width_ratio_max = 5.0) {
std::vector<Cone> cones;
std::cout << "=== 锥桶识别开始 ===" << std::endl;
std::cout << "锥桶识别参数: 高度[" << min_height << ", " << max_height
<< "], 宽度[" << min_width << ", " << max_width
<< "], 高宽比[" << height_to_width_ratio_min << ", " << height_to_width_ratio_max << "]" << std::endl;
// 使用欧式距离聚类
auto clusters = EuclideanClustering(cloud, 0.15, 10, 1000);
int cone_id = 0;
int potential_cones = 0;
for (size_t i = 0; i < clusters.size(); ++i) {
auto cluster = clusters[i];
// 计算边界框
Box box = BoundingBox(cluster);
float height = box.z_max - box.z_min;
float width_x = box.x_max - box.x_min;
float width_y = box.y_max - box.y_min;
float width = std::max(width_x, width_y);
// 计算高度与宽度的比例
float height_to_width_ratio = (width > 0) ? height / width : 0;
// 计算中心点
pcl::PointXYZ center;
center.x = (box.x_min + box.x_max) / 2.0;
center.y = (box.y_min + box.y_max) / 2.0;
center.z = (box.z_min + box.z_max) / 2.0;
// 计算置信度
float confidence = 0.0;
// 检查尺寸是否符合锥桶特征
bool height_ok = (height >= min_height && height <= max_height);
bool width_ok = (width >= min_width && width <= max_width);
bool ratio_ok = (height_to_width_ratio >= height_to_width_ratio_min &&
height_to_width_ratio <= height_to_width_ratio_max);
std::cout << "聚类 " << i << " 分析: " << std::endl;
std::cout << " - 位置: (" << std::fixed << std::setprecision(2)
<< center.x << ", " << center.y << ", " << center.z << ")" << std::endl;
std::cout << " - 尺寸: 高度=" << height << "m, 宽度=" << width << "m, 高宽比=" << height_to_width_ratio << std::endl;
std::cout << " - 检查结果: 高度" << (height_ok ? "✓" : "✗")
<< " 宽度" << (width_ok ? "✓" : "✗")
<< " 高宽比" << (ratio_ok ? "✓" : "✗") << std::endl;
if (height_ok && width_ok && ratio_ok) {
potential_cones++;
// 计算置信度
confidence = 0.7f; // 基础置信度
if (height_ok) confidence += 0.1f;
if (width_ok) confidence += 0.1f;
if (ratio_ok) confidence += 0.1f;
// 确保置信度不超过1.0
confidence = std::min(confidence, 1.0f);
Cone cone;
cone.center = center;
cone.height = height;
cone.width = width;
cone.confidence = confidence;
cone.id = cone_id++;
cone.type = "unknown";
cones.push_back(cone);
std::cout << ">>> 识别为锥桶 #" << cone.id << " 置信度: " << confidence << std::endl;
} else {
std::cout << ">>> 未识别为锥桶" << std::endl;
}
}
std::cout << "锥桶识别完成,共检测到 " << cones.size() << " 个锥桶 ("
<< potential_cones << " 个潜在锥桶)" << std::endl;
return cones;
}
};
// 主要的ROS2节点类
class EuclideanClusteringNode : public rclcpp::Node {
public:
EuclideanClusteringNode() : Node("euclidean_clustering_node") {
// 声明参数 - 使用更宽松的默认值
this->declare_parameter("cluster_tolerance", 0.09);
this->declare_parameter("min_cluster_size", 10);
this->declare_parameter("max_cluster_size", 200);
this->declare_parameter("downsample_leaf_size", 0.03f);
this->declare_parameter("enable_downsampling", false);
// 锥桶识别参数 - 放宽条件
this->declare_parameter("cone_min_height", 0.0);
this->declare_parameter("cone_max_height", 0.8);
this->declare_parameter("cone_min_width", 0.0);
this->declare_parameter("cone_max_width", 6.0);
this->declare_parameter("cone_min_ratio", 0.0);
this->declare_parameter("cone_max_ratio", 6.0);
this->declare_parameter("cone_confidence_threshold", 0.3);
// 直通滤波参数
this->declare_parameter("enable_passthrough", false);
this->declare_parameter("passthrough_x_min", -10.0);
this->declare_parameter("passthrough_x_max", 10.0);
this->declare_parameter("passthrough_y_min", -10.0);
this->declare_parameter("passthrough_y_max", 10.0);
this->declare_parameter("passthrough_z_min", -0.5);
this->declare_parameter("passthrough_z_max", 2.0);
// 创建订阅者
subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
"/filtered_pointcloud", 10,
std::bind(&EuclideanClusteringNode::pointcloud_callback, this, std::placeholders::_1));
// 创建发布者
cluster_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("clustering_results/clusters", 10);
cone_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("clustering_results/cones", 10);
marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("clustering_results/markers", 10);
marker_id_counter_ = 0;
RCLCPP_INFO(this->get_logger(), "欧式距离聚类节点已初始化");
}
private:
void debug_pointcloud_stats(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::string& name) {
if (cloud->empty()) {
std::cout << name << ": 空点云" << std::endl;
return;
}
pcl::PointXYZ min_pt, max_pt;
pcl::getMinMax3D(*cloud, min_pt, max_pt);
std::cout << name << "统计:" << std::endl;
std::cout << " - 点数: " << cloud->size() << std::endl;
std::cout << " - 范围 X: " << min_pt.x << " 到 " << max_pt.x << std::endl;
std::cout << " - 范围 Y: " << min_pt.y << " 到 " << max_pt.y << std::endl;
std::cout << " - 范围 Z: " << min_pt.z << " 到 " << max_pt.z << std::endl;
// 统计有效点
int valid_points = 0;
for (const auto& point : cloud->points) {
if (isPointFinite(point)) valid_points++;
}
std::cout << " - 有效点: " << valid_points << "/" << cloud->size() << std::endl;
}
void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
auto start_time = std::chrono::steady_clock::now();
std::cout << "\n\n=== 收到新点云 ===" << std::endl;
std::cout << "时间戳: " << msg->header.stamp.sec << "." << msg->header.stamp.nanosec << std::endl;
std::cout << "坐标系: " << msg->header.frame_id << std::endl;
// 转换点云格式
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*msg, *cloud);
debug_pointcloud_stats(cloud, "原始点云");
if (cloud->empty()) {
RCLCPP_WARN(this->get_logger(), "收到空点云");
return;
}
// 获取参数
float cluster_tolerance = this->get_parameter("cluster_tolerance").as_double();
int min_cluster_size = this->get_parameter("min_cluster_size").as_int();
int max_cluster_size = this->get_parameter("max_cluster_size").as_int();
float downsample_leaf_size = this->get_parameter("downsample_leaf_size").as_double();
bool enable_downsampling = this->get_parameter("enable_downsampling").as_bool();
// 获取锥桶识别参数
float cone_min_height = this->get_parameter("cone_min_height").as_double();
float cone_max_height = this->get_parameter("cone_max_height").as_double();
float cone_min_width = this->get_parameter("cone_min_width").as_double();
float cone_max_width = this->get_parameter("cone_max_width").as_double();
float cone_min_ratio = this->get_parameter("cone_min_ratio").as_double();
float cone_max_ratio = this->get_parameter("cone_max_ratio").as_double();
float cone_confidence_threshold = this->get_parameter("cone_confidence_threshold").as_double();
// 获取直通滤波参数
bool enable_passthrough = this->get_parameter("enable_passthrough").as_bool();
float passthrough_x_min = this->get_parameter("passthrough_x_min").as_double();
float passthrough_x_max = this->get_parameter("passthrough_x_max").as_double();
float passthrough_y_min = this->get_parameter("passthrough_y_min").as_double();
float passthrough_y_max = this->get_parameter("passthrough_y_max").as_double();
float passthrough_z_min = this->get_parameter("passthrough_z_min").as_double();
float passthrough_z_max = this->get_parameter("passthrough_z_max").as_double();
// 打印实际使用的参数
std::cout << "使用参数:" << std::endl;
std::cout << " - 聚类: 容差=" << cluster_tolerance << ", 最小=" << min_cluster_size
<< ", 最大=" << max_cluster_size << std::endl;
std::cout << " - 锥桶: 高度[" << cone_min_height << ", " << cone_max_height
<< "], 宽度[" << cone_min_width << ", " << cone_max_width
<< "], 高宽比[" << cone_min_ratio << ", " << cone_max_ratio << "]" << std::endl;
// 创建点云处理器
ProcessPointClouds<pcl::PointXYZ> processor;
// 1. 预处理:移除无效点
auto filtered_cloud = processor.RemoveNaNPoints(cloud);
debug_pointcloud_stats(filtered_cloud, "过滤后点云");
if (filtered_cloud->empty()) {
RCLCPP_WARN(this->get_logger(), "过滤后点云为空");
return;
}
// 2. 可选:直通滤波(限制感兴趣区域)
pcl::PointCloud<pcl::PointXYZ>::Ptr processed_cloud = filtered_cloud;
if (enable_passthrough) {
std::cout << "应用直通滤波..." << std::endl;
// X方向滤波
processed_cloud = processor.PassThroughFilter(processed_cloud, "x", passthrough_x_min, passthrough_x_max);
debug_pointcloud_stats(processed_cloud, "X方向滤波后");
// Y方向滤波
processed_cloud = processor.PassThroughFilter(processed_cloud, "y", passthrough_y_min, passthrough_y_max);
debug_pointcloud_stats(processed_cloud, "Y方向滤波后");
// Z方向滤波
processed_cloud = processor.PassThroughFilter(processed_cloud, "z", passthrough_z_min, passthrough_z_max);
debug_pointcloud_stats(processed_cloud, "Z方向滤波后");
}
// 3. 可选:降采样点云
if (enable_downsampling) {
processed_cloud = processor.DownsampleCloud(processed_cloud, downsample_leaf_size);
debug_pointcloud_stats(processed_cloud, "降采样后点云");
}
if (processed_cloud->empty()) {
RCLCPP_WARN(this->get_logger(), "处理后的点云为空");
return;
}
// 4. 欧式距离聚类
auto clusters = processor.EuclideanClustering(processed_cloud, cluster_tolerance,
min_cluster_size, max_cluster_size);
std::cout << "聚类结果: " << clusters.size() << " 个聚类" << std::endl;
for (size_t i = 0; i < clusters.size(); ++i) {
Box box = processor.BoundingBox(clusters[i]);
std::cout << " 聚类 " << i << ": " << clusters[i]->size() << " 个点, "
<< "位置(" << std::fixed << std::setprecision(2)
<< (box.x_min + box.x_max)/2 << ", "
<< (box.y_min + box.y_max)/2 << ", "
<< (box.z_min + box.z_max)/2 << ")" << std::endl;
}
// 5. 锥桶识别
auto cones = processor.DetectCones(processed_cloud,
cone_min_height, cone_max_height,
cone_min_width, cone_max_width,
cone_min_ratio, cone_max_ratio);
// 6. 创建可视化结果
visualization_msgs::msg::MarkerArray all_markers;
pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_clusters_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cone_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
// 预定义颜色 - 紫色用于聚类点云
std::vector<uint8_t> purple_color = {128, 0, 128}; // 紫色
std::vector<uint8_t> green_color = {0, 255, 0}; // 绿色用于标记
// 处理聚类结果
for (size_t i = 0; i < clusters.size(); ++i) {
bool is_cone = false;
Cone matched_cone;
if (!clusters[i]->empty()) {
Box cluster_box = processor.BoundingBox(clusters[i]);
pcl::PointXYZ cluster_center;
cluster_center.x = (cluster_box.x_min + cluster_box.x_max) / 2.0;
cluster_center.y = (cluster_box.y_min + cluster_box.y_max) / 2.0;
cluster_center.z = (cluster_box.z_min + cluster_box.z_max) / 2.0;
// 匹配锥桶
for (const auto& cone : cones) {
float dist = processor.EuclideanDistance(cluster_center, cone.center);
if (dist < 0.2 && cone.confidence >= cone_confidence_threshold) {
is_cone = true;
matched_cone = cone;
break;
}
}
}
// 分配颜色 - cluster_pub_使用紫色,cone_pub_使用橙色
std::vector<uint8_t> cluster_color = purple_color; // 聚类点云使用紫色
std::vector<uint8_t> cone_color = {255, 165, 0}; // 锥桶点云使用橙色
// 创建彩色点云
for (const auto& point : clusters[i]->points) {
if (isPointFinite(point)) {
// 为cluster_pub_创建紫色点云
pcl::PointXYZRGB colored_point;
colored_point.x = point.x;
colored_point.y = point.y;
colored_point.z = point.z;
colored_point.r = cluster_color[0];
colored_point.g = cluster_color[1];
colored_point.b = cluster_color[2];
colored_clusters_cloud->points.push_back(colored_point);
// 如果是锥桶,为cone_pub_创建橙色点云
if (is_cone) {
pcl::PointXYZRGB cone_colored_point;
cone_colored_point.x = point.x;
cone_colored_point.y = point.y;
cone_colored_point.z = point.z;
cone_colored_point.r = cone_color[0];
cone_colored_point.g = cone_color[1];
cone_colored_point.b = cone_color[2];
cone_cloud->points.push_back(cone_colored_point);
}
}
}
// 创建边界框标记 - 使用绿色
Box box = processor.BoundingBox(clusters[i]);
visualization_msgs::msg::Marker bbox_marker;
bbox_marker.header = msg->header;
bbox_marker.ns = "bounding_boxes";
bbox_marker.id = marker_id_counter_++;
bbox_marker.type = visualization_msgs::msg::Marker::CUBE;
bbox_marker.action = visualization_msgs::msg::Marker::ADD;
bbox_marker.pose.position.x = (box.x_min + box.x_max) / 2.0;
bbox_marker.pose.position.y = (box.y_min + box.y_max) / 2.0;
bbox_marker.pose.position.z = (box.z_min + box.z_max) / 2.0;
bbox_marker.pose.orientation.w = 1.0;
bbox_marker.scale.x = box.x_max - box.x_min;
bbox_marker.scale.y = box.y_max - box.y_min;
bbox_marker.scale.z = box.z_max - box.z_min;
// 标记使用绿色
bbox_marker.color.r = green_color[0] / 255.0;
bbox_marker.color.g = green_color[1] / 255.0;
bbox_marker.color.b = green_color[2] / 255.0;
bbox_marker.color.a = 0.3;
bbox_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
all_markers.markers.push_back(bbox_marker);
}
// 创建锥桶文本标记 - 使用绿色
for (size_t i = 0; i < cones.size(); ++i) {
const auto& cone = cones[i];
if (cone.confidence >= cone_confidence_threshold) {
visualization_msgs::msg::Marker text_marker;
text_marker.header = msg->header;
text_marker.ns = "cone_text";
text_marker.id = marker_id_counter_++;
text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
text_marker.action = visualization_msgs::msg::Marker::ADD;
text_marker.pose.position.x = cone.center.x;
text_marker.pose.position.y = cone.center.y;
text_marker.pose.position.z = cone.center.z + cone.height + 0.15;
text_marker.scale.z = 0.15;
// 文本标记使用绿色
text_marker.color.r = green_color[0] / 255.0;
text_marker.color.g = green_color[1] / 255.0;
text_marker.color.b = green_color[2] / 255.0;
text_marker.color.a = 1.0;
std::stringstream coord_stream;
coord_stream << std::fixed << std::setprecision(2);
coord_stream << "x: " << cone.center.x << "\n";
coord_stream << "y: " << cone.center.y << "\n";
coord_stream << "z: " << cone.center.z;
text_marker.text = "Cone " + std::to_string(cone.id) +
" (" + std::to_string(static_cast<int>(cone.confidence * 100)) + "%)\n" +
coord_stream.str();
text_marker.lifetime = rclcpp::Duration::from_seconds(0.5);
all_markers.markers.push_back(text_marker);
}
}
// 发布所有结果
publish_colored_pointcloud(colored_clusters_cloud, msg->header, cluster_pub_);
publish_colored_pointcloud(cone_cloud, msg->header, cone_pub_);
marker_pub_->publish(all_markers);
auto end_time = std::chrono::steady_clock::now();
auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time - start_time);
RCLCPP_INFO(this->get_logger(), "处理完成: %lu 个聚类, %lu 个锥桶, 耗时 %ld ms",
clusters.size(), cones.size(), elapsed_time.count());
// 重置标记ID计数器
if (marker_id_counter_ > 10000) {
marker_id_counter_ = 0;
}
}
void publish_pointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
const std_msgs::msg::Header& header,
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher) {
if (cloud->empty()) return;
sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
output.header = header;
output.header.frame_id = header.frame_id.empty() ? "rslidar" : header.frame_id;
output.header.stamp = this->now();
publisher->publish(output);
}
void publish_colored_pointcloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& cloud,
const std_msgs::msg::Header& header,
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr publisher) {
if (cloud->empty()) return;
sensor_msgs::msg::PointCloud2 output;
pcl::toROSMsg(*cloud, output);
output.header = header;
output.header.frame_id = header.frame_id.empty() ? "rslidar" : header.frame_id;
output.header.stamp = this->now();
publisher->publish(output);
}
// 订阅者和发布者
rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cone_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
int marker_id_counter_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<EuclideanClusteringNode>();
RCLCPP_INFO(node->get_logger(), "启动欧式距离聚类节点");
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}
这个代码存在那些问题
最新发布