fix bug

src/perception/rslidar_pointcloud_merger/GPU移植说明.md

@@ -1,289 +0,0 @@
-# GPU加速点云处理 - Orin平台移植指南
-
-## 概述
-
-本项目支持在 NVIDIA Jetson Orin 平台上使用 GPU 加速点云处理，提供了性能显著提升的版本。
-
-## 硬件要求
-
-- NVIDIA Jetson Orin (Nano / TX2 / Xavier / AGX)
-- CUDA 11.0 或更高版本
-- JetPack 5.0 或更高版本
-
-## 1. Orin 平台准备
-
-### 1.1 检查 CUDA 和 JetPack 版本
-
-在 Orin 上运行以下命令检查版本：
-
-```bash
-# 检查 JetPack 版本
-cat /etc/nv_tegra_release
-
-# 检查 CUDA 版本
-nvcc -V
-
-# 检查 GPU 状态
-nvidia-smi
-```
-
-### 1.2 验证 CUDA 编译环境
-
-确保 CUDA 编译器可用：
-
-```bash
-which nvcc
-ls -la /usr/local/cuda/
-```
-
-如果没有找到 CUDA，可能需要重新安装 JetPack。
-
-## 2. 编译项目
-
-### 2.1 配置编译参数
-
-在 Orin 上使用 GPU 加速编译：
-
-```bash
-cd /home/ubuntu/project/zxwl/sweeper/sweeper_200
-
-# 清理之前的编译（可选）
-rm -rf build/ install/ log/
-
-# 使用 GPU 加速编译（注意使用 --cmake-args 传递参数）
-colcon build --packages-select rslidar_pointcloud_merger \
-    --cmake-args -DUSE_GPU=ON \
-    --event-handlers console_direct+
-```
-
-### 2.2 检查编译结果
-
-```bash
-# 检查安装的文件
-ls -la install/rslidar_pointcloud_merger/lib/rslidar_pointcloud_merger/
-ls -la install/rslidar_pointcloud_merger/share/rslidar_pointcloud_merger/
-```
-
-### 2.3 如果编译失败（常见问题）
-
-**问题 1: CUDA 未找到**
-```
-CMake Error: Could NOT find CUDA (missing: CUDA_INCLUDE_DIRS)
-```
-解决方法：确保 CUDA 路径正确，或检查 CUDA 安装。
-
-**问题 2: 架构不匹配**
-```
-nvcc fatal : Unsupported gpu architecture 'compute_30'
-```
-解决方法：修改 `CMakeLists.txt` 中的 `CUDA_ARCHITECTURES` 设置。
-
-## 3. 运行
-
-### 3.1 配置运行参数
-
-在 launch 文件中添加 `use_gpu:=true` 参数：
-
-```bash
-# 创建新的 launch 文件或修改 existing_launch.launch.py
-ros2 launch rslidar_pointcloud_merger merge_two_lidars.launch.py use_gpu:=true
-```
-
-### 3.2 直接命令行运行（测试用）
-
-```bash
-cd /home/ubuntu/project/zxwl/sweeper/sweeper_200
-
-# 配置环境
-source install/setup.bash
-
-# 运行（需要启用 use_gpu 选项）
-ros2 run rslidar_pointcloud_merger merge_two_lidars --ros-args \
-    -p use_gpu:=true
-```
-
-### 3.3 性能测试
-
-```bash
-# 在运行过程中查看资源使用情况
-tegrastats
-```
-
-## 4. 性能比较
-
-| 功能 | CPU 版本 | GPU 版本 (Orin AGX) | 性能提升 |
-|------|----------|---------------------|----------|
-| 条件过滤 | 0.15s/帧 | 0.03s/帧 | 5x |
-| 体素降采样 | 0.20s/帧 | 0.02s/帧 | 10x |
-| 离群点去除 | 0.35s/帧 | 0.05s/帧 | 7x |
-| 总处理时间 | 0.70s/帧 | 0.10s/帧 | 7x |
-
-**注意**：性能提升因数据规模和 Orin 型号而异。
-
-## 5. 验证结果
-
-### 5.1 检查输出话题
-
-```bash
-# 查看节点和话题
-ros2 node list
-ros2 topic list
-
-# 打印话题内容（可选）
-ros2 topic echo /rslidar_points
-ros2 topic echo /grid_raw
-```
-
-### 5.2 使用 Rviz 可视化
-
-```bash
-ros2 run rviz2 rviz2
-```
-
-配置：
-1. Add → By topic → /rslidar_points → PointCloud2
-2. 在 Displays 面板中设置 Fixed Frame 为 "rslidar"
-3. 如果看到点云显示且栅格正常，说明 GPU 加速正常工作
-
-## 6. 故障排除
-
-### 6.1 节点无法启动
-
-**现象：**
-```bash
-[ERROR] [merge_two_lidars]: Failed to load nodelet
-[ERROR] [merge_two_lidars]: GPU not available
-```
-
-**解决方法：**
-- 检查是否启用了 USE_GPU 编译参数
-- 检查 CUDA 库是否正确加载
-- 检查 `/usr/local/cuda/lib64` 是否在 LD_LIBRARY_PATH 中
-
-### 6.2 运行时崩溃
-
-**现象：**
-```bash
-[merge_two_lidars]: Segmentation fault (core dumped)
-```
-
-**解决方法：**
-- 检查是否启用了正确的架构（compute_87 for Orin AGX）
-- 增加 swap 空间
-- 使用 `gdb` 查看详细的堆栈跟踪
-
-### 6.3 性能不符合预期
-
-**现象：**
-- GPU 使用率低（nvidia-smi 显示 <20%）
-- 处理延迟高
-
-**解决方法：**
-- 优化线程配置
-- 增加批处理大小
-- 检查数据传输瓶颈
-
-## 7. 编译优化选项
-
-### 7.1 针对特定 Orin 型号优化
-
-修改 `CMakeLists.txt`：
-
-```cmake
-if(USE_GPU)
-  # Orin AGX (compute_87)
-  set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -arch=sm_87 -O3")
-endif()
-```
-
-| Orin 型号 | 架构代码 |
-|-----------|----------|
-| Nano | sm_72 |
-| TX2 | sm_62 |
-| Xavier | sm_72 |
-| AGX Orin | sm_87 |
-
-### 7.2 启用编译器优化
-
-```cmake
-if(USE_GPU)
-  set(CMAKE_CUDA_FLAGS "${CMAKE_CUDA_FLAGS} -O3 -lineinfo")
-endif()
-```
-
-## 8. 代码结构说明
-
-### 8.1 文件变更
-
-**新增文件：**
-- `src/gpu_processing.cu` - GPU 加速的点云处理算法（CUDA 实现）
-- `include/gpu_processing.h` - GPU 处理函数声明
-
-**修改文件：**
-- `CMakeLists.txt` - 添加 CUDA 支持
-- `src/merge_two_lidars.cpp` - 添加 GPU 调用逻辑
-
-### 8.2 GPU/CPU 切换机制
-
-代码会自动检查 GPU 是否可用：
-- 如果可用且编译了 GPU 支持 → 使用 GPU 处理
-- 否则 → 自动降级到 CPU 实现（保持兼容性）
-
-## 9. 未来优化方向
-
-### 9.1 优化数据传输
-
-```cpp
-// 优化建议：使用零拷贝内存
-void optimizeMemoryTransfer()
-{
-  // 使用 CUDA Host Registered Memory
-  cudaHostAlloc(...);
-
-  // 异步数据传输
-  cudaMemcpyAsync(...);
-}
-```
-
-### 9.2 使用 TensorRT
-
-```cpp
-// 优化建议：使用 TensorRT 进行推断
-void useTensorRT()
-{
-  // 加载优化后的引擎
-  nvinfer1::ICudaEngine* engine = createEngine(...);
-
-  // 执行 GPU 推理
-  engine->execute(...);
-}
-```
-
-### 9.3 多线程优化
-
-```cpp
-// 优化建议：使用 CUDA 流
-void useCudaStreams()
-{
-  cudaStream_t stream;
-  cudaStreamCreate(&stream);
-
-  // 异步操作
-  kernel<<<blocks, threads, 0, stream>>>(...);
-
-  cudaStreamSynchronize(stream);
-  cudaStreamDestroy(stream);
-}
-```
-
-## 10. 参考资料
-
-- [NVIDIA Jetson 开发文档](https://docs.nvidia.com/jetson/index.html)
-- [CUDA 编程指南](https://docs.nvidia.com/cuda/cuda-c-programming-guide/index.html)
-- [PCL CUDA 扩展](https://pointclouds.org/documentation/tutorials/gpu.html)
-- [GPU 加速计算教程](https://developer.nvidia.com/accelerated-computing-guide)
-
----
-
-**维护者：** Your Name
-**最后更新：** 2025年1月

src/perception/rslidar_pointcloud_merger/include/gpu_processing.h

@@ -15,6 +15,13 @@ struct float3
     float3(float x_, float y_, float z_) : x(x_), y(y_), z(z_) {}
 };
 
+struct int3
+{
+    int x, y, z;
+    int3() : x(0), y(0), z(0) {}
+    int3(int x_, int y_, int z_) : x(x_), y(y_), z(z_) {}
+};
+
 // 检查GPU是否可用
 bool isDeviceAvailable();
 
@@ -29,9 +36,9 @@ std::vector<float3> gpuVoxelGridFilter(const std::vector<float>& points, float r
 std::vector<int> gpuStatisticalOutlierRemoval(const std::vector<float>& points, int mean_k, float std_thresh);
 
 // 辅助函数：将PCL点云转换为简单浮点数组
-std::vector<float> convertToFloatArray(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
+std::vector<float> convertToFloatArray(const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>& cloud);
 
 // 辅助函数：将浮点数组转换为PCL点云
-pcl::PointCloud<pcl::PointXYZ>::Ptr convertToPCLCloud(const std::vector<float>& points);
+boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> convertToPCLCloud(const std::vector<float>& points);
 
 #endif  // GPU_PROCESSING_H

src/perception/rslidar_pointcloud_merger/src/gpu_processing.cu

@@ -2,6 +2,16 @@
 
 #include <vector>
 
+#include "gpu_processing.h"
+
+// CUDA头文件
+#ifdef USE_GPU
+#include <cuda_runtime.h>
+#endif
+
+// Boost头文件
+#include <boost/shared_ptr.hpp>
+
 // 定义__device__和__host__函数
 #ifdef __CUDACC__
 #define DEVICE_FUNC __device__ __host__
@@ -323,9 +333,11 @@ std::vector<int> gpuStatisticalOutlierRemoval(const std::vector<float>& points,
 }
 
 // 辅助函数：将PCL点云转换为简单浮点数组
-std::vector<float> convertToFloatArray(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
+std::vector<float> convertToFloatArray(const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>& cloud)
 {
     std::vector<float> result;
+    if (!cloud) return result;
+
     result.reserve(cloud->size() * 3);
 
     for (const auto& point : cloud->points)
@@ -339,7 +351,7 @@ std::vector<float> convertToFloatArray(const pcl::PointCloud<pcl::PointXYZ>::Ptr
 }
 
 // 辅助函数：将浮点数组转换为PCL点云
-pcl::PointCloud<pcl::PointXYZ>::Ptr convertToPCLCloud(const std::vector<float>& points)
+boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> convertToPCLCloud(const std::vector<float>& points)
 {
     auto cloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ>>();
     cloud->resize(points.size() / 3);