1. 基于LiDAR点云的障碍物检测与聚类

任务目标

本案例基于Autoware的激光雷达驱动，完成从原始点云到障碍物检测结果的完整闭环：对Velodyne VLP-16采集的点云进行体素降采样与直通滤波，剔除地面点后执行欧几里得聚类，为每个障碍物簇生成轴对齐包围框，并输出可视化结果。

环境与依赖

  • Ubuntu 20.04 + ROS 2 Foxy（或对应Autoware版本）
  • Autoware Universe（已安装autoware_auto_perception相关组件）
  • PCL 1.10+、Open3D（仅用于离线验证，非运行时依赖）
  • 测试数据：从autoware_data下载示例rosbag（含/points_raw话题）

核心实现

以下节点订阅原始点云话题，依次执行：体素降采样（leaf size 0.1m）→ 直通滤波（保留前方20m、左右10m、高度-1~2m范围）→ 基于RANSAC的地面分割 → 欧几里得聚类（距离阈值0.5m，最小簇50点）→ 生成包围框并发布。代码为最小可复现实现，不包含工程化封装。

```cpp #include #include #include #include #include #include #include #include #include #include #include #include class ObstacleClusterNode : public rclcpp::Node { public: ObstacleClusterNode() : Node("obstacle_cluster_node") { sub_ = create_subscription( "/points_raw", 10, [this](sensor_msgs::msg::PointCloud2::SharedPtr msg) { processCloud(msg); }); pub_clusters_ = create_publisher("/clusters", 10); pub_boxes_ = create_publisher("/bboxes", 10); } private: void processCloud(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { pcl::PointCloud::Ptr raw(new pcl::PointCloud()); pcl::fromROSMsg(*msg, *raw); // 1. 体素降采样 pcl::VoxelGrid voxel; voxel.setInputCloud(raw); voxel.setLeafSize(0.1f, 0.1f, 0.1f); pcl::PointCloud::Ptr downsampled(new pcl::PointCloud()); voxel.filter(*downsampled); // 2. 直通滤波（前方20m，左右10m，高度-1~2m） pcl::PassThrough pass; pass.setInputCloud(downsampled); pass.setFilterFieldName("x"); pass.setFilterLimits(0.0, 20.0); pcl::PointCloud::Ptr x_filtered(new pcl::PointCloud()); pass.filter(*x_filtered); pass.setInputCloud(x_filtered); pass.setFilterFieldName("y"); pass.setFilterLimits(-10.0, 10.0); pcl::PointCloud::Ptr xy_filtered(new pcl::PointCloud()); pass.filter(*xy_filtered); pass.setInputCloud(xy_filtered); pass.setFilterFieldName("z"); pass.setFilterLimits(-1.0, 2.0); pcl::PointCloud::Ptr xyz_filtered(new pcl::PointCloud()); pass.filter(*xyz_filtered); // 3. RANSAC地面分割 pcl::SACSegmentation seg; seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_PLANE); seg.setMethodType(pcl::SAC_RANSAC); seg.setDistanceThreshold(0.2); seg.setMaxIterations(100); pcl::PointIndices::Ptr inliers(new pcl::PointIndices()); pcl::ModelCoefficients::Ptr coeffs(new pcl::ModelCoefficients()); seg.setInputCloud(xyz_filtered); seg.segment(*inliers, *coeffs); pcl::ExtractIndices extract; extract.setInputCloud(xyz_filtered); extract.setIndices(inliers); extract.setNegative(true); pcl::PointCloud::Ptr obstacles(new pcl::PointCloud()); extract.filter(*obstacles); // 4. 欧几里得聚类 pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); tree->setInputCloud(obstacles); std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(0.5); ec.setMinClusterSize(50); ec.setMaxClusterSize(100000); ec.setSearchMethod(tree); ec.setInputCloud(obstacles); ec.extract(cluster_indices); // 5. 合并所有簇点云并生成包围框 pcl::PointCloud::Ptr all_clusters(new pcl::PointCloud()); visualization_msgs::msg::MarkerArray boxes; int cluster_id = 0; for (const auto& indices : cluster_indices) { pcl::PointCloud::Ptr cluster(new pcl::PointCloud()); for (int idx : indices.indices) cluster->points.push_back(obstacles->points[idx]); cluster->width = cluster->points.size(); cluster->height = 1; cluster->is_dense = true; *all_clusters += *cluster; // 计算AABB包围框 Eigen::Vector4f min_pt, max_pt; pcl::getMinMax3D(*cluster, min_pt, max_pt); visualization_msgs::msg::Marker box; box.header = msg->header; box.ns = "obstacles"; box.id = cluster_id++; box.type = visualization_msgs::msg::Marker::CUBE; box.action = visualization_msgs::msg::Marker::ADD; box.pose.position.x = (min_pt[0] + max_pt[0]) / 2.0; box.pose.position.y = (min_pt[1] + max_pt[1]) / 2.0; box.pose.position.z = (min_pt[2] + max_pt[2]) / 2.0; box.scale.x = max_pt[0] - min_pt[0]; box.scale.y = max_pt[1] - min_pt[1]; box.scale.z = max_pt[2] - min_pt[2]; box.color.r = 1.0f; box.color.g = 0.0f; box.color.b = 0.0f; box.color.a = 0.6f; boxes.markers.push_back(box); } // 发布结果 sensor_msgs::msg::PointCloud2 out_msg; pcl::toROSMsg(*all_clusters, out_msg); out_msg.header = msg->header; pub_clusters_->publish(out_msg); pub_boxes_->publish(boxes); } rclcpp::Subscription::SharedPtr sub_; rclcpp::Publisher::SharedPtr pub_clusters_; rclcpp::Publisher::SharedPtr pub_boxes_; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } ```

编译与运行

  1. 将上述代码保存为obstacle_cluster_node.cpp，放入Autoware感知包的src/目录，并在CMakeLists.txt中添加：
```cmake find_package(PCL REQUIRED) find_package(visualization_msgs REQUIRED) add_executable(obstacle_cluster_node src/obstacle_cluster_node.cpp) ament_target_dependencies(obstacle_cluster_node rclcpp sensor_msgs visualization_msgs) target_link_libraries(obstacle_cluster_node ${PCL_LIBRARIES}) install(TARGETS obstacle_cluster_node DESTINATION lib/${PROJECT_NAME}) ```
  1. 编译并运行：
```bash colcon build --packages-select your_package source install/setup.bash ros2 run your_package obstacle_cluster_node ```

验证方法

在另一个终端播放测试数据并观察结果：

```bash ros2 bag play your_data.db3 rviz2 # 添加PointCloud2话题/clusters和MarkerArray话题/bboxes ```

通过条件：

  • RViz中/clusters显示为去除地面后的障碍物点云簇，颜色与原始点云可区分；
  • /bboxes在车辆前方障碍物（行人、车辆、锥桶）周围显示红色半透明立方体，每个障碍物一个框，框体紧密贴合点云簇边界；
  • 终端无报错，节点持续输出且帧率与输入点云话题一致（约10Hz）。

常见问题

  • 无聚类输出：检查setMinClusterSize(50)是否过大——若场景中障碍物较小，可降至20；确认直通滤波范围未将目标区域完全滤除。
  • 地面未完全去除：RANSAC距离阈值0.2m在颠簸路面可能残留地面点，可降至0.15m或改用SACMODEL_NORMAL_PLANE。
  • 包围框抖动：体素leaf size增大至0.2m可减少噪声，但会降低小障碍物检测精度，需根据场景权衡。
← 上一章 📖 返回目录 下一章 →