diff --git a/src/perception/rslidar_pointcloud_merger/CMakeLists.txt b/src/perception/rslidar_pointcloud_merger/CMakeLists.txt index 6bb154b..ac39714 100644 --- a/src/perception/rslidar_pointcloud_merger/CMakeLists.txt +++ b/src/perception/rslidar_pointcloud_merger/CMakeLists.txt @@ -22,7 +22,7 @@ find_package(pcl_ros REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) -find_package(PCL REQUIRED COMPONENTS common io) +find_package(PCL REQUIRED COMPONENTS common io segmentation filters) # GPU加速相关配置 if(USE_GPU) @@ -79,6 +79,6 @@ endif() install(TARGETS merge_two_lidars DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/src/perception/rslidar_pointcloud_merger/config/merger_config.yaml b/src/perception/rslidar_pointcloud_merger/config/merger_config.yaml new file mode 100644 index 0000000..80f1ce8 --- /dev/null +++ b/src/perception/rslidar_pointcloud_merger/config/merger_config.yaml @@ -0,0 +1,67 @@ +lidar_merger: + ros__parameters: + # 基本参数 + front_topic: "/rslidar_points/front_lidar" + rear_topic: "/rslidar_points/rear_lidar" + output_topic: "/rslidar_points" + frame_id: "rslidar" + queue_size: 3 + time_tolerance: 0.1 + enable_debug: false + + # 点云范围过滤 + filter_min_x: -5.0 + filter_max_x: 10.0 + filter_min_y: -5.0 + filter_max_y: 5.0 + filter_min_z: 0.0 + filter_max_z: 1.0 + + # 体素降采样 + voxel_size: 0.1 + + # 离群点去除 (SOR) + stat_mean_k: 30 + stat_std_thresh: 1.5 + sor_method: 1 + radius_search: 0.5 + min_neighbors: 3 + + # 地面分割 (PMF) + enable_ground_segmentation: true + ground_max_window_size: 32 + ground_slope: 1.0 + ground_initial_distance: 0.3 + ground_max_distance: 2.5 + + # 栅格可视化 + grid_size: 50 + grid_range: 15.0 + enable_print: false + + # GPU + use_gpu: true + + # 车身过滤 + filter_car: true + car_length: 1.8 + car_width: 1.34 + car_lidar_offset_x: 0.0 + car_lidar_offset_y: 0.0 + + # 扫刷过滤 + filter_brush: true + # 右扫刷 + brush_r_min_x: 0.85 + brush_r_max_x: 1.25 + brush_r_min_y: 0.60 + brush_r_max_y: 0.96 + brush_r_min_z: 1.20 + brush_r_max_z: 1.65 + # 左扫刷 + brush_l_min_x: 0.85 + brush_l_max_x: 1.25 + brush_l_min_y: -0.96 + brush_l_max_y: -0.60 + brush_l_min_z: 1.20 + brush_l_max_z: 1.65 diff --git a/src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py b/src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py index 79d128e..0a5088e 100644 --- a/src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py +++ b/src/perception/rslidar_pointcloud_merger/launch/merge_two_lidars.launch.py @@ -6,6 +6,7 @@ from pathlib import Path def generate_launch_description(): pkg_share = get_package_share_directory("rslidar_pointcloud_merger") + config_file = Path(pkg_share) / "config" / "merger_config.yaml" # front_lidar（右前角） -> rslidar（车辆中心） # [x, y, z, yaw, pitch, roll] @@ -37,7 +38,6 @@ def generate_launch_description(): executable="static_transform_publisher", name="static_tf_front", arguments=[*map(str, tf_front), "rslidar", "front_lidar"], - # arguments=[*map(str, tf_front), "rslidar", "front_lidar", "100"], output="log", ), # ---------- 静态 TF (rear) ---------- @@ -46,7 +46,6 @@ def generate_launch_description(): executable="static_transform_publisher", name="static_tf_rear", arguments=[*map(str, tf_rear), "rslidar", "rear_lidar"], - # arguments=[*map(str, tf_rear), "rslidar", "rear_lidar", "100"], output="log", ), # ---------- 点云合并节点 ---------- @@ -55,41 +54,7 @@ def generate_launch_description(): executable="merge_two_lidars", name="lidar_merger", parameters=[ - { - "front_topic": "/rslidar_points/front_lidar", - "rear_topic": "/rslidar_points/rear_lidar", - "output_topic": "/rslidar_points", - "frame_id": "rslidar", - "queue_size": 3, - "cache_size": 10, - "time_tolerance": 0.1, - "max_wait_time": 1.0, - "enable_debug": False, - # 点云处理参数 - "filter_min_x": -5.0, # X轴最小坐标（米） - "filter_max_x": 10.0, # X轴最大坐标（米） - "filter_min_y": -5.0, # Y轴最小坐标（米） - "filter_max_y": 5.0, # Y轴最大坐标（米） - "filter_min_z": 0.0, # Z轴最小坐标（米） - "filter_max_z": 1.0, # Z轴最大坐标（米）1.0 - "voxel_size": 0.1, # 体素网格大小（米） 0.1~0.3m - "stat_mean_k": 30, # 计算点的平均距离时考虑的邻居数 - "stat_std_thresh": 1.5, # 标准差倍数阈值 噪声较多 ：0.5~1.0。 噪声较少 ：1.0~2.0。 - "grid_size": 50, # 栅格矩阵的边长（单元格数） - "grid_range": 15.0, # 栅格覆盖的实际空间范围（米） - # 单元格尺寸：由 grid_range / grid_size 决定 - "enable_print": False, # 是否打印栅格 - "use_gpu": True, # 是否使用GPU加速 - "sor_method": 1, # 推荐先用1，效果不满意再试2 - "radius_search": 0.5, # 搜索半径（米），越大越严格 - "min_neighbors": 3, # 至少需要N个邻居才保留 - # 车身过滤参数 - "filter_car": True, # 是否启用车身过滤 - "car_length": 1.8, # 车长（米） - "car_width": 1.34, # 车宽（米） - "car_lidar_offset_x": 0.0, # LiDAR在x轴的安装偏移 - "car_lidar_offset_y": 0.0, # LiDAR在y轴的安装偏移 - } + config_file, ], output="screen", ), diff --git a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp index a8054d6..3fbfc73 100644 --- a/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp +++ b/src/perception/rslidar_pointcloud_merger/src/merge_two_lidars.cpp @@ -26,11 +26,13 @@ */ #include +#include #include #include #include #include #include +#include #include #include #include @@ -211,6 +213,28 @@ class LidarMerger : public rclcpp::Node car_lidar_offset_x_ = declare_parameter("car_lidar_offset_x", 0.0f); car_lidar_offset_y_ = declare_parameter("car_lidar_offset_y", 0.0f); + // 扫刷过滤 + filter_brush_ = declare_parameter("filter_brush", true); + brush_r_min_x_ = declare_parameter("brush_r_min_x", 0.85f); + brush_r_max_x_ = declare_parameter("brush_r_max_x", 1.25f); + brush_r_min_y_ = declare_parameter("brush_r_min_y", 0.60f); + brush_r_max_y_ = declare_parameter("brush_r_max_y", 0.96f); + brush_r_min_z_ = declare_parameter("brush_r_min_z", 1.20f); + brush_r_max_z_ = declare_parameter("brush_r_max_z", 1.65f); + brush_l_min_x_ = declare_parameter("brush_l_min_x", 0.85f); + brush_l_max_x_ = declare_parameter("brush_l_max_x", 1.25f); + brush_l_min_y_ = declare_parameter("brush_l_min_y", -0.96f); + brush_l_max_y_ = declare_parameter("brush_l_max_y", -0.60f); + brush_l_min_z_ = declare_parameter("brush_l_min_z", 1.20f); + brush_l_max_z_ = declare_parameter("brush_l_max_z", 1.65f); + + // 地面分割 + enable_ground_segmentation_ = declare_parameter("enable_ground_segmentation", true); + ground_max_window_size_ = declare_parameter("ground_max_window_size", 32); + ground_slope_ = declare_parameter("ground_slope", 1.0); + ground_initial_distance_ = declare_parameter("ground_initial_distance", 0.3); + ground_max_distance_ = declare_parameter("ground_max_distance", 2.5); + /* ---- 预计算栅格参数 ---- */ grid_resolution_ = grid_range_ / static_cast(grid_size_); grid_inv_resolution_ = 1.0f / grid_resolution_; @@ -687,9 +711,13 @@ class LidarMerger : public rclcpp::Node { if (fx >= cmin_x && fx <= cmax_x && fy >= cmin_y && fy <= cmax_y) continue; - if (fx >= 0.85f && fx <= 1.25f && fy >= 0.60f && fy <= 0.96f && fz >= 1.20f && fz <= 1.65f) continue; - - if (fx >= 0.85f && fx <= 1.25f && fy >= -0.96f && fy <= -0.60f && fz >= 1.20f && fz <= 1.65f) continue; + if (filter_brush_) + { + // 右扫刷 + if (fx >= brush_r_min_x_ && fx <= brush_r_max_x_ && fy >= brush_r_min_y_ && fy <= brush_r_max_y_ && fz >= brush_r_min_z_ && fz <= brush_r_max_z_) continue; + // 左扫刷 + if (fx >= brush_l_min_x_ && fx <= brush_l_max_x_ && fy >= brush_l_min_y_ && fy <= brush_l_max_y_ && fz >= brush_l_min_z_ && fz <= brush_l_max_z_) continue; + } } pcl::PointXYZ pt; @@ -709,8 +737,35 @@ class LidarMerger : public rclcpp::Node vg_local.filter(*downsampled); if (downsampled->empty()) return nullptr; - /* 3. 离群点去除（根据 sor_method_ 选择算法） */ - pcl_out = applyOutlierRemoval(downsampled); + /* 3. 地面分割 (PMF) */ + pcl::PointCloud::Ptr non_ground(new pcl::PointCloud); + if (enable_ground_segmentation_) + { + pcl::PointIndicesPtr ground_indices(new pcl::PointIndices); + pcl::ProgressiveMorphologicalFilter pmf; + pmf.setInputCloud(downsampled); + pmf.setMaxWindowSize(ground_max_window_size_); + pmf.setSlope(ground_slope_); + pmf.setInitialDistance(ground_initial_distance_); + pmf.setMaxDistance(ground_max_distance_); + pmf.extract(ground_indices->indices); + + pcl::ExtractIndices extractor; + extractor.setInputCloud(downsampled); + extractor.setIndices(ground_indices); + extractor.setNegative(true); // 提取非地面 + extractor.filter(*non_ground); + } + else + { + // 不进行地面分割，直接使用体素化后的点云 + non_ground = downsampled; + } + + if (non_ground->empty()) return nullptr; + + /* 4. 离群点去除（根据 sor_method_ 选择算法） */ + pcl_out = applyOutlierRemoval(non_ground); if (!pcl_out || pcl_out->empty()) return nullptr; /* 4. 直接构建 PointCloud2（不走 toROSMsg，减少一次拷贝） */ @@ -1035,19 +1090,19 @@ class LidarMerger : public rclcpp::Node p.car_max_y = car_lidar_offset_y_ + car_width_ * 0.5f; p.car_min_z = filter_min_z_; p.car_max_z = filter_max_z_; - p.filter_brush = true; - p.brush_r_min_x = 0.85f; - p.brush_r_max_x = 1.25f; - p.brush_r_min_y = 0.60f; - p.brush_r_max_y = 0.96f; - p.brush_r_min_z = 1.20f; - p.brush_r_max_z = 1.65f; - p.brush_l_min_x = 0.85f; - p.brush_l_max_x = 1.25f; - p.brush_l_min_y = -0.96f; - p.brush_l_max_y = -0.60f; - p.brush_l_min_z = 1.20f; - p.brush_l_max_z = 1.65f; + p.filter_brush = filter_brush_; + p.brush_r_min_x = brush_r_min_x_; + p.brush_r_max_x = brush_r_max_x_; + p.brush_r_min_y = brush_r_min_y_; + p.brush_r_max_y = brush_r_max_y_; + p.brush_r_min_z = brush_r_min_z_; + p.brush_r_max_z = brush_r_max_z_; + p.brush_l_min_x = brush_l_min_x_; + p.brush_l_max_x = brush_l_max_x_; + p.brush_l_min_y = brush_l_min_y_; + p.brush_l_max_y = brush_l_max_y_; + p.brush_l_min_z = brush_l_min_z_; + p.brush_l_max_z = brush_l_max_z_; } return p; } @@ -1101,12 +1156,20 @@ class LidarMerger : public rclcpp::Node int sor_method_, min_neighbors_; float radius_search_; double time_tolerance_; - bool enable_debug_, enable_print_, use_gpu_, filter_car_; + bool enable_debug_, enable_print_, use_gpu_, filter_car_, filter_brush_, enable_ground_segmentation_; float filter_min_x_, filter_max_x_, filter_min_y_, filter_max_y_; float filter_min_z_, filter_max_z_; float voxel_size_, stat_std_thresh_, grid_range_; float car_length_, car_width_, car_lidar_offset_x_, car_lidar_offset_y_; float grid_resolution_, grid_inv_resolution_; + // 扫刷参数 + float brush_r_min_x_, brush_r_max_x_, brush_r_min_y_, brush_r_max_y_, brush_r_min_z_, brush_r_max_z_; + float brush_l_min_x_, brush_l_max_x_, brush_l_min_y_, brush_l_max_y_, brush_l_min_z_, brush_l_max_z_; + // 地面分割参数 (PMF) + int ground_max_window_size_; + double ground_slope_; + double ground_initial_distance_; + double ground_max_distance_; /* -- PCL 过滤器（仅作为模板，处理线程使用线程本地副本） -- */ pcl::VoxelGrid vg_;