#
include <rcl
cpp/rcl
cpp.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 rcl
cpp::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;
RCL
CPP_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()) {
RCL
CPP_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()) {
RCL
CPP_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()) {
RCL
CPP_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 = rcl
cpp::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 = rcl
cpp::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);
RCL
CPP_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,
rcl
cpp::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,
rcl
cpp::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);
}
// 订阅者和发布者
rcl
cpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
rcl
cpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cluster_pub_;
rcl
cpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr cone_pub_;
rcl
cpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
int marker_id_counter_;
};
int main(int argc, char** argv) {
rcl
cpp::init(argc, argv);
auto node = std::make_shared<EuclideanClusteringNode>();
RCL
CPP_INFO(node->get_logger(), "启动欧式距离聚类节点");
rcl
cpp::spin(node);
rcl
cpp::shutdown();
return 0;
}
这个代码存在那些问题
本文展示了一个使用C++编写的简单程序，该程序通过异常处理机制来避免除数为零的情况。当用户输入的除数为零时，程序会捕获并处理这个异常，输出错误信息。
扫一扫
1232
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
