update

launch/run.launch

@@ -0,0 +1,4 @@
+<launch>
+<node pkg="rviz"  type="rviz"  name="rviz"  args="-d $(find guangpo_lidar)/rviz/guangpo.rviz"></node>
+<node pkg="guangpo_lidar"  type="guangpo_lidar_node"  name="guangpo_lidar_node"  output="screen" ></node>
+</launch>

rviz/guangpo.rviz

@@ -0,0 +1,208 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /MarkerArray1/Status1
+        - /PointCloud22
+      Splitter Ratio: 0.5
+    Tree Height: 257
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: Image
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /usb_cam/image_raw
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Axes
+      Enabled: true
+      Length: 1
+      Name: Axes
+      Radius: 0.10000000149011612
+      Reference Frame: <Fixed Frame>
+      Show Trail: false
+      Value: true
+    - Class: rviz/MarkerArray
+      Enabled: true
+      Marker Topic: /obstacles_topic
+      Name: MarkerArray
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /cloud_ground
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 2.202476739883423
+        Min Value: 0.08952760696411133
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /cloud_noground
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: world
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 19.902732849121094
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.344796061515808
+      Target Frame: <Fixed Frame>
+      Yaw: 3.2617619037628174
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 740
+  Hide Left Dock: false
+  Hide Right Dock: false
+  Image:
+    collapsed: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000245fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003e0000018c000000cb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001d0000000b30000001600ffffff000000010000010f00000245fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003e00000245000000a500fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005e80000003efc0100000002fb0000000800540069006d00650100000000000005e80000033700fffffffb0000000800540069006d00650100000000000004500000000000000000000003770000024500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1512
+  X: 0
+  Y: 0

src/cluster3d.cpp

@@ -49,4 +49,4 @@ std::vector<PtCdtr<PointT>> ClusterPts<PointT>::EuclidCluster(PtCdtr<PointT> clo
         }
     }
     return clusters;
-}
+}

src/environment.cpp

@@ -126,17 +126,6 @@ void initCamera(CameraAngle setAngle, pcl::visualization::PCLVisualizer::Ptr &vi
         viewer->addCoordinateSystem(1.0);
 }
 
-// 计算旋转矩阵  
-Eigen::Matrix4d TransforMatrix(Eigen::Vector3d translation,double roll,double pitch,double yaw) {
-    Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())  
-                         * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())  
-                         * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());  
-    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵  
-    transformation_matrix.block<3, 3>(0, 0) = q.toRotationMatrix(); // 设置旋转矩阵部分  
-    transformation_matrix.block<3, 1>(0, 3) = translation; // 设置平移向量部分  
-    std::cout << "旋转矩阵：" << std::endl << transformation_matrix << std::endl;  
-    return transformation_matrix;
-}
 
 // char* argv[] means array of char pointers, whereas char** argv means pointer to a char pointer.
 int main(int argc, char **argv) {

src/main_ros.cpp

@@ -1,11 +1,15 @@
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <pcl/PCLPointCloud2.h>
+#include <pcl/point_types.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud2.h>
-#include <pcl_conversions/pcl_conversions.h>
-#include <pcl/point_types.h>
-#include <pcl/PCLPointCloud2.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
 
-#include "render/render.h"
 #include "processPointClouds.h"
+#include "render/render.h"
 // using templates for processPointClouds so also include .cpp to help linker
 #include "processPointClouds.cpp"
 
@@ -15,55 +19,402 @@
 
 using namespace lidar_obstacle_detection;
 
+template <typename PointT = pcl::PointXYZI>
+std::pair<PtCdtr<PointT>, PtCdtr<PointT>>
+RansacSegmentPlane(PtCdtr<PointT> cloud, int maxIterations, float distanceTol) {
+  // 使用pcl的分割，分离出地面
+  // Count time
+  auto startTime = std::chrono::steady_clock::now();
+
+  // Create the segmentation object
+  pcl::SACSegmentation<PointT> seg;
+
+  // Optional
+  seg.setOptimizeCoefficients(true);
+  seg.setModelType(pcl::SACMODEL_PLANE);
+  seg.setMethodType(pcl::SAC_RANSAC);
+  seg.setMaxIterations(maxIterations);
+  seg.setDistanceThreshold(distanceTol);
+
+  // Segment the largest planar component
+  // PtCdtr<PointT> cloud_plane(new pcl::PointCloud<PointT>());
+  // PtCdtr<PointT> cloud_out(new pcl::PointCloud<PointT>());
+  pcl::ModelCoefficients::Ptr cloud_out(new pcl::ModelCoefficients);
+  pcl::PointIndices::Ptr cloud_plane(new pcl::PointIndices);
+
+  seg.setInputCloud(cloud);
+  seg.segment(*cloud_plane, *cloud_out);
+
+  PtCdtr<PointT> in_plane(new pcl::PointCloud<PointT>());
+  PtCdtr<PointT> out_plane(new pcl::PointCloud<PointT>());
+
+  // Extract the inliers
+  pcl::ExtractIndices<PointT> extract;
+  extract.setInputCloud(cloud);
+  extract.setIndices(cloud_plane);
+  extract.setNegative(false);
+  extract.filter(*in_plane);
+
+  // Extract the outliers
+  extract.setNegative(true);
+  extract.filter(*out_plane);
+  auto endTime = std::chrono::steady_clock::now();
+  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      endTime - startTime);
+  std::cout << "plane ransac-segment took " << elapsedTime.count()
+            << " milliseconds" << std::endl;
+
+  bool if_view_segment = yaml_config["view"]["segment"].as<bool>();
+  if (if_view_segment) {
+    // 创建可视化窗口
+    pcl::visualization::PCLVisualizer::Ptr viewer(
+        new pcl::visualization::PCLVisualizer("RANSAC Plane Segmentation"));
+    viewer->setWindowName("My Plane Segmentation Viewer");
+    viewer->setBackgroundColor(0, 0, 0);
+
+    // 添加坐标系
+    viewer->addCoordinateSystem(1.0);
+    // 添加栅格
+    for (float x = -10.0; x <= 10.0; x += 1.0) {
+      viewer->addLine(pcl::PointXYZ(x, -10, 0), pcl::PointXYZ(x, 10, 0), 0.5,
+                      0.5, 0.5, std::string("grid_") + std::to_string(x) + "y");
+    }
+    for (float y = -10.0; y <= 10.0; y += 1.0) {
+      viewer->addLine(pcl::PointXYZ(-10, y, 0), pcl::PointXYZ(10, y, 0), 0.5,
+                      0.5, 0.5, std::string("grid_") + std::to_string(y) + "x");
+    }
+
+    // 可视化地面点云
+    pcl::visualization::PointCloudColorHandlerCustom<PointT> plane_color(
+        in_plane, 255, 0, 0);
+    viewer->addPointCloud<PointT>(in_plane, plane_color, "plane");
+    viewer->setPointCloudRenderingProperties(
+        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");
+
+    // 可视化非地面点云
+    pcl::visualization::PointCloudColorHandlerCustom<PointT> non_plane_color(
+        out_plane, 0, 255, 0);
+    viewer->addPointCloud<PointT>(out_plane, non_plane_color, "non_plane");
+    viewer->setPointCloudRenderingProperties(
+        pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "non_plane");
+
+    // 显示可视化窗口
+    while (!viewer->wasStopped()) {
+      viewer->spinOnce();
+    }
+  }
+  return std::make_pair(out_plane, in_plane);
+}
+
+template <typename PointT = pcl::PointXYZI>
+pcl::PointCloud<pcl::PointXYZI>::Ptr
+FilterCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud, float filterRes,
+            const Eigen::Vector4f &minPoint, const Eigen::Vector4f &maxPoint) {
+
+  // Time segmentation process
+  auto startTime = std::chrono::steady_clock::now();
+
+  // TODO:: Fill in the function to do voxel grid point reduction and region
+  // based filtering Create the filtering object: downsample the dataset using a
+  // leaf size of .2m 体素降采样
+  pcl::VoxelGrid<PointT> vg;
+  PtCdtr<PointT> cloudFiltered(new pcl::PointCloud<PointT>);
+  vg.setInputCloud(cloud);
+  vg.setLeafSize(filterRes, filterRes, filterRes);
+  vg.filter(*cloudFiltered);
+
+  // 设置roi区域
+  PtCdtr<PointT> cloudRegion(new pcl::PointCloud<PointT>);
+  pcl::CropBox<PointT> region(true);
+  region.setMin(minPoint);
+  region.setMax(maxPoint);
+  region.setInputCloud(cloudFiltered);
+  region.filter(*cloudRegion);
+
+  // 去除屋顶点云
+  std::vector<int> indices;
+  pcl::CropBox<PointT> roof(true);
+  // roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
+  // roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
+  double max_x = yaml_config["roof"]["max_x"].as<double>();
+  double max_y = yaml_config["roof"]["max_y"].as<double>();
+  double max_z = yaml_config["roof"]["max_z"].as<double>();
+  double min_x = yaml_config["roof"]["min_x"].as<double>();
+  double min_y = yaml_config["roof"]["min_y"].as<double>();
+  double min_z = yaml_config["roof"]["min_z"].as<double>();
+  roof.setMin(Eigen::Vector4f(min_x, min_y, min_z, 1));
+  roof.setMax(Eigen::Vector4f(max_x, max_y, max_z, 1));
+  roof.setInputCloud(cloudRegion);
+  roof.filter(indices);
+
+  pcl::PointIndices::Ptr inliers{new pcl::PointIndices};
+  for (int point : indices) {
+    inliers->indices.push_back(point);
+  }
+  pcl::ExtractIndices<PointT> extract;
+  extract.setInputCloud(cloudRegion);
+  extract.setIndices(inliers);
+  extract.setNegative(true);
+  extract.filter(*cloudRegion);
+
+  auto endTime = std::chrono::steady_clock::now();
+  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      endTime - startTime);
+  std::cout << "filtering took " << elapsedTime.count() << " milliseconds"
+            << std::endl;
+
+  //    return cloud;
+  return cloudRegion;
+}
 
 class PCLSubscriber {
 public:
-    PCLSubscriber(ros::NodeHandle& nh) {
-        std::string sub_topic=yaml_config["sub_cloud_topic"].as<std::string>();
-        std::cout << "订阅点云话题" << sub_topic << std::endl;
-        std::string pub_topic=yaml_config["pub_cloud_topic"].as<std::string>();
-        std::cout << "发布点云话题" << pub_topic << std::endl;
-        sub_ = nh.subscribe<sensor_msgs::PointCloud2>(sub_topic, 10, &PCLSubscriber::pointCloudCallback, this);
-        pub_ = nh.advertise<sensor_msgs::PointCloud2>(pub_topic, 1);
-    }
+  PCLSubscriber(ros::NodeHandle &nh) {
+    std::string sub_topic = yaml_config["sub_cloud_topic"].as<std::string>();
+    std::cout << "订阅点云话题" << sub_topic << std::endl;
+    std::string pub_cloud_ground_topic =
+        yaml_config["pub_cloud_ground_topic"].as<std::string>();
+    std::cout << "发布地面点云话题" << pub_cloud_ground_topic << std::endl;
+    std::string pub_cloud_noground_topic =
+        yaml_config["pub_cloud_noground_topic"].as<std::string>();
+    std::cout << "发布非地面点云话题" << pub_cloud_ground_topic << std::endl;
+    std::string markder_pub_topic =
+        yaml_config["obstacles_topic"].as<std::string>();
+    std::cout << "发布障碍物可视化话题" << markder_pub_topic << std::endl;
+    sub_ = nh.subscribe<sensor_msgs::PointCloud2>(
+        sub_topic, 10, &PCLSubscriber::pointCloudCallback, this);
+    pub_cloud_ground_ =
+        nh.advertise<sensor_msgs::PointCloud2>(pub_cloud_ground_topic, 10);
+    pub_cloud_noground_ =
+        nh.advertise<sensor_msgs::PointCloud2>(pub_cloud_noground_topic, 10);
+    marker_pub_ =
+        nh.advertise<visualization_msgs::MarkerArray>(markder_pub_topic, 10);
 
-    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg) {
-        pcl::PCLPointCloud2 pcl_pc2;
-        pcl_conversions::toPCL(*msg, pcl_pc2);
+    // 初始化坐标系变换的旋转矩阵
+    Eigen::Vector3d trans;
+    trans[0] = yaml_config["convert"]["transform"]["x"].as<double>();
+    trans[1] = yaml_config["convert"]["transform"]["y"].as<double>();
+    trans[2] = yaml_config["convert"]["transform"]["z"].as<double>();
+    std::cout << "trans x=" << trans[0] << std::endl;
+    std::cout << "trans y=" << trans[1] << std::endl;
+    std::cout << "trans z=" << trans[2] << std::endl;
+    // trans<<0.3643,0.2078,1.21;
+    double roll = yaml_config["convert"]["rotation"]["roll"].as<double>();
+    double pitch = yaml_config["convert"]["rotation"]["pitch"].as<double>();
+    double yaw = yaml_config["convert"]["rotation"]["yaw"].as<double>();
+    std::cout << "roll=" << roll << std::endl;
+    std::cout << "pitch=" << pitch << std::endl;
+    std::cout << "yaw=" << yaw << std::endl;
+    Eigen::Matrix4d tran_matrix = TransforMatrix(
+        trans, roll * M_PI / 180.0, pitch * M_PI / 180.0, yaw * M_PI / 180.0);
+    Eigen::Affine3d affine3d_transform(tran_matrix); // 转换为Affine3d类型
+    affine3f_transform = affine3d_transform.cast<float>(); // 转换为Affine3f类型
+    affine3f_transform = affine3f_transform.inverse();
+  }
 
-        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
-        pcl::fromPCLPointCloud2(pcl_pc2, *cloud);
+  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg) {
+    pcl::PCLPointCloud2 pcl_pc2;
+    pcl_conversions::toPCL(*msg, pcl_pc2);
 
-        processPointCloud(cloud);
-        publishCloud(cloud);
-    }
+    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(
+        new pcl::PointCloud<pcl::PointXYZI>);
+    pcl::fromPCLPointCloud2(pcl_pc2, *cloud);
+
+    processPointCloud(cloud);
+  }
 
 private:
-    void processPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
-        // 在这里添加对点云数据的处理逻辑
-        ROS_INFO("收到包含 %lu 个点的点云数据", cloud->size());
+  void processPointCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    // 在这里添加对点云数据的处理逻辑
+    ROS_INFO("收到包含 %lu 个点的点云数据", cloud->size());
+
+    // 将点云进行转换
+    pcl::transformPointCloud(*cloud, *cloud, affine3f_transform);
+
+    auto startTime = std::chrono::steady_clock::now();
+    // 对点云进行处理
+    DealWithCloud(cloud);
+    auto endTime = std::chrono::steady_clock::now();
+    auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+        endTime - startTime);
+    std::cout << "All operators took " << elapsedTime.count()
+              << " milliseconds" << std::endl;
+  }
+
+  // Test read Lidar data
+  void DealWithCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud) {
+    // ----------------------------------------------------
+    // -----Open 3D viewer and display City Block     -----
+    // ----------------------------------------------------
+
+    // Setting hyper parameters
+
+    // FilterCloud
+    // float filterRes = 0.4;
+    float filterRes = yaml_config["filterRes"].as<float>();
+    // Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
+    // Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
+    double max_x = yaml_config["roi"]["max_x"].as<double>();
+    double max_y = yaml_config["roi"]["max_y"].as<double>();
+    double max_z = yaml_config["roi"]["max_z"].as<double>();
+    double min_x = yaml_config["roi"]["min_x"].as<double>();
+    double min_y = yaml_config["roi"]["min_y"].as<double>();
+    double min_z = yaml_config["roi"]["min_z"].as<double>();
+    Eigen::Vector4f maxpoint(max_x, max_y, max_z, 1);
+    Eigen::Vector4f minpoint(min_x, min_y, min_z, 1);
+    // SegmentPlane
+    // int maxIterations = 40;
+    int maxIterations = yaml_config["ransac"]["maxIterations"].as<int>();
+    // float distanceThreshold = 0.3;
+    float distanceThreshold =
+        yaml_config["ransac"]["distanceThreshold"].as<float>();
+    // 聚类参数设置
+    // Clustering
+    // float clusterTolerance = 0.5;
+    // int minsize = 10;
+    // int maxsize = 140;
+    float clusterTolerance =
+        yaml_config["cluster"]["clusterTolerance"].as<float>();
+    int minsize = yaml_config["cluster"]["minsize"].as<int>();
+    int maxsize = yaml_config["cluster"]["maxsize"].as<int>();
+
+    // 1. 降采样+设置roi区域+去除车辆相关点云
+    // First:Filter cloud to reduce amount of points
+    pcl::PointCloud<pcl::PointXYZI>::Ptr filteredCloud =
+        FilterCloud(inputCloud, filterRes, minpoint, maxpoint);
+    // 2. 把点云分离出路面
+    // Second: Segment the filtered cloud into obstacles and road
+    std::pair<pcl::PointCloud<pcl::PointXYZI>::Ptr,
+              pcl::PointCloud<pcl::PointXYZI>::Ptr>
+        segmentCloud =
+            RansacSegmentPlane(filteredCloud, maxIterations, distanceThreshold);
+
+    // 发布点云
+    ROS_INFO("发布点云cloud");
+    PublishGroundCloud(segmentCloud.second);
+    PublishNoGroundCloud(segmentCloud.first);
+    //    renderPointCloud(viewer, segmentCloud.first, "obstCloud", Color(1, 0,
+    //    0));
+    // renderPointCloud(viewer, segmentCloud.second, "planeCloud", Color(0, 1,
+    // 0));
+    //    renderPointCloud(viewer,inputCloud,"inputCloud");
+    // Third: Cluster different obstacle cloud
+    // std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters =
+    // pointProcessorI->EuclideanClustering(segmentCloud.first,
+    // clusterTolerance, minsize, maxsize); 提取出障碍物的点云
+    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> cloudClusters =
+        GxtEuclideanClustering(segmentCloud.first, clusterTolerance, minsize,
+                               maxsize);
+
+    std::cout << "begin ExtractObstacles" << std::endl;
+    std::vector<Eigen::Vector3f> centroids;
+    std::vector<Eigen::Vector3f> sizes;
+    std::vector<Eigen::Quaternionf> orientations;
+    bool enable_verbose = yaml_config["log"]["enable_obstacles"].as<bool>();
+    // 根据障碍物点云提取出框
+    ExtractObstacles(cloudClusters, centroids, sizes, orientations,
+                     enable_verbose);
+    // visualizeObstacles(cloudClusters, centroids, sizes, orientations);
+
+    // int clusterId = 0;
+    // std::vector<Color> colors = {Color(1, 0, 0), Color(0, 1, 0), Color(0, 0,
+    // 1)};
+
+    // for (pcl::PointCloud<pcl::PointXYZI>::Ptr cluster : cloudClusters) {
+    //
+    //   std::cout << "cluster size";
+    //   pointProcessorI->numPoints(cluster);
+    //   renderPointCloud(viewer, cluster, "obstCLoud" +
+    //   std::to_string(clusterId),
+    //                    colors[clusterId % colors.size()]);
+    //   // Fourth: Find bounding boxes for each obstacle cluster
+    //   Box box = pointProcessorI->BoundingBox(cluster);
+    //   renderBox(viewer, box, clusterId);
+    //   ++clusterId;
+    // }
+    PublishObstacles(centroids, sizes, orientations);
+  }
+
+  void PublishGroundCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    // 将 pcl::PointCloud 转换为 sensor_msgs::PointCloud2
+    sensor_msgs::PointCloud2 msg;
+    pcl::toROSMsg(*cloud, msg);
+    msg.header.frame_id = "world";
+    msg.header.stamp = ros::Time::now();
+
+    pub_cloud_ground_.publish(msg);
+  }
+  void PublishNoGroundCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
+    // 将 pcl::PointCloud 转换为 sensor_msgs::PointCloud2
+    sensor_msgs::PointCloud2 msg;
+    pcl::toROSMsg(*cloud, msg);
+    msg.header.frame_id = "world";
+    msg.header.stamp = ros::Time::now();
+
+    pub_cloud_noground_.publish(msg);
+  }
+
+  void PublishObstacles(
+      const std::vector<Eigen::Vector3f> &obstacle_centroids,
+      const std::vector<Eigen::Vector3f> &obstacle_sizes,
+      const std::vector<Eigen::Quaternionf> &obstacle_orientations) {
+
+    visualization_msgs::MarkerArray marker_array;
+    // 首先发布一个DELETEALL命令,删除上次发布的所有障碍物标记
+    visualization_msgs::Marker delete_marker;
+    delete_marker.header.frame_id = "world";
+    delete_marker.action = visualization_msgs::Marker::DELETEALL;
+    marker_array.markers.push_back(delete_marker);
+    marker_pub_.publish(marker_array);
+    marker_array.markers.clear(); // 清空marker_array
+
+    for (size_t i = 0; i < obstacle_centroids.size(); ++i) {
+      visualization_msgs::Marker marker;
+      marker.header.frame_id = "world";
+      marker.type = visualization_msgs::Marker::CUBE;
+      marker.action = visualization_msgs::Marker::ADD;
+      marker.id = i;
+
+      marker.pose.position.x = obstacle_centroids[i].x();
+      marker.pose.position.y = obstacle_centroids[i].y();
+      marker.pose.position.z = obstacle_centroids[i].z();
+
+      marker.pose.orientation.x = obstacle_orientations[i].x();
+      marker.pose.orientation.y = obstacle_orientations[i].y();
+      marker.pose.orientation.z = obstacle_orientations[i].z();
+      marker.pose.orientation.w = obstacle_orientations[i].w();
+
+      marker.scale.x = obstacle_sizes[i].x();
+      marker.scale.y = obstacle_sizes[i].y();
+      marker.scale.z = obstacle_sizes[i].z();
+
+      marker.color.r = 1.0f;
+      marker.color.g = 0.0f;
+      marker.color.b = 0.0f;
+      marker.color.a = 0.3;
+
+      marker_array.markers.push_back(marker);
     }
 
-    void publishCloud(const pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud) {
-        // 将 pcl::PointCloud 转换为 sensor_msgs::PointCloud2
-        sensor_msgs::PointCloud2 msg;
-        pcl::toROSMsg(*cloud, msg);
-        msg.header.frame_id = "world";
-        msg.header.stamp = ros::Time::now();
+    marker_pub_.publish(marker_array);
+  }
+  ros::Publisher marker_pub_;
 
-        pub_.publish(msg);
-    }
+  ros::Subscriber sub_;
+  ros::Publisher pub_cloud_noground_;
+  ros::Publisher pub_cloud_ground_;
 
-    ros::Subscriber sub_;
-    ros::Publisher pub_;
+  // 激光雷达到车体坐标系的变换
+  Eigen::Affine3f affine3f_transform;
 };
 
-int main(int argc, char** argv) {
-    ros::init(argc, argv, "pcl_subscriber");
-    ros::NodeHandle nh;
+int main(int argc, char **argv) {
+  setlocale(LC_ALL, "");
+  ros::init(argc, argv, "pcl_subscriber");
+  ros::NodeHandle nh;
 
-    PCLSubscriber subscriber(nh);
+  PCLSubscriber subscriber(nh);
 
-    ros::spin();
-    return 0;
+  ros::spin();
+  return 0;
 }

src/obstacles.hpp

@@ -1,29 +1,27 @@
 #pragma once
 
-#include <pcl/point_types.h>
+#include <pcl/common/transforms.h>
 #include <pcl/filters/extract_indices.h>
+#include <pcl/point_types.h>
 #include <pcl/segmentation/extract_clusters.h>
 #include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/common/transforms.h>
 
-#include <pcl/common/pca.h>
-#include <pcl/point_types.h>
-#include <pcl/filters/extract_indices.h>
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/visualization/pcl_visualizer.h>
-#include <pcl/common/transforms.h>
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h> 
-#include <pcl/filters/filter.h>
-#include <pcl/filters/voxel_grid.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/kdtree/kdtree.h>
-#include <pcl/segmentation/sac_segmentation.h> 
-#include <pcl/segmentation/extract_clusters.h>
-#include <pcl/common/common.h>
-#include <pcl/common/eigen.h>
 #include <Eigen/Dense>
 #include <iostream>
+#include <pcl/common/common.h>
+#include <pcl/common/eigen.h>
+#include <pcl/common/pca.h>
+#include <pcl/common/transforms.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/filter.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/kdtree/kdtree.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/visualization/pcl_visualizer.h>
 #include <vector>
 
 #include <pcl/features/moment_of_inertia_estimation.h>
@@ -31,7 +29,81 @@
 #include <pcl/point_types.h>
 #include <pcl/visualization/cloud_viewer.h>
 
-// reference https://pointclouds.org/documentation/tutorials/moment_of_inertia.html
+/**
+ * 使用欧式聚类对点云进行分割
+ * @param input_cloud 输入点云
+ * @param cluster_tolerance 聚类距离阈值(单位:米)
+ * @param min_cluster_size 最小聚类尺寸
+ * @param max_cluster_size 最大聚类尺寸
+ * @param output_clouds 输出聚类结果点云向量
+ */
+template <typename PointT = pcl::PointXYZI>
+std::vector<typename pcl::PointCloud<PointT>::Ptr>
+GxtEuclideanClustering(typename pcl::PointCloud<PointT>::Ptr input_cloud,
+                       float cluster_tolerance, int min_cluster_size,
+                       int max_cluster_size) {
+  auto startTime = std::chrono::steady_clock::now();
+  // 创建欧式聚类对象
+  pcl::EuclideanClusterExtraction<PointT> ec;
+  ec.setClusterTolerance(cluster_tolerance);
+  ec.setMinClusterSize(min_cluster_size);
+  ec.setMaxClusterSize(max_cluster_size);
+  ec.setSearchMethod(pcl::search::KdTree<pcl::PointXYZI>::Ptr(
+      new pcl::search::KdTree<PointT>));
+
+  // 提取聚类
+  std::vector<pcl::PointIndices> cluster_indices;
+  ec.setInputCloud(input_cloud);
+  ec.extract(cluster_indices);
+
+  std::vector<typename pcl::PointCloud<PointT>::Ptr> output_clouds;
+  // 生成聚类结果点云
+  output_clouds.clear();
+  for (const auto &indices : cluster_indices) {
+    typename pcl::PointCloud<PointT>::Ptr cluster(new pcl::PointCloud<PointT>);
+    for (int index : indices.indices) {
+      cluster->push_back(input_cloud->at(index));
+    }
+    output_clouds.push_back(cluster);
+  }
+  auto endTime = std::chrono::steady_clock::now();
+  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      endTime - startTime);
+  std::cout << "euclidean clustering took " << elapsedTime.count() << " milliseconds"
+            << std::endl;
+  if (false) {
+    // 可视化聚类结果
+    pcl::visualization::PCLVisualizer viewer("Euclidean Clustering");
+    viewer.setWindowName("My Plane Segmentation Viewer");
+    viewer.setBackgroundColor(0, 0, 0);
+    int j = 0;
+    for (const auto &indices : cluster_indices) {
+      pcl::PointCloud<pcl::PointXYZI>::Ptr cluster(
+          new pcl::PointCloud<pcl::PointXYZI>);
+      for (int index : indices.indices) {
+        cluster->push_back(input_cloud->at(index));
+      }
+      viewer.addPointCloud<pcl::PointXYZI>(cluster,
+                                           "cluster_" + std::to_string(j++));
+      viewer.setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2,
+          "cluster_" + std::to_string(j - 1));
+      viewer.setPointCloudRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_COLOR,
+          (float)rand() / (float)RAND_MAX, (float)rand() / (float)RAND_MAX,
+          (float)rand() / (float)RAND_MAX, "cluster_" + std::to_string(j - 1));
+    }
+    viewer.spin();
+    // // 显示可视化窗口
+    // while (!viewer.wasStopped()) {
+    //   viewer.spinOnce();
+    // }
+  }
+  return output_clouds;
+}
+
+// reference
+// https://pointclouds.org/documentation/tutorials/moment_of_inertia.html
 /**
  * 从聚类结果中提取障碍物信息
  * @param obstacle_clouds 输出的障碍物点云集合
@@ -39,28 +111,40 @@
  * @param obstacle_sizes 输出的障碍物尺寸集合
  * @param obstacle_orientations 输出的障碍物旋转集合
  */
-void ExtractObstacles(
-    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& obstacle_clouds,
-    std::vector<Eigen::Vector3f>& obstacle_centroids,
-    std::vector<Eigen::Vector3f>& obstacle_sizes,
-    std::vector<Eigen::Quaternionf>& obstacle_orientations)
-{
-    obstacle_centroids.clear();
-    obstacle_sizes.clear();
-    obstacle_orientations.clear();
-    for (const auto &obstacle_cloud : obstacle_clouds) {
-      pcl::MomentOfInertiaEstimation<pcl::PointXYZI> feature_extractor;
-      feature_extractor.setInputCloud(obstacle_cloud);
-      feature_extractor.compute();
+inline void ExtractObstacles(
+    std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> &obstacle_clouds,
+    std::vector<Eigen::Vector3f> &obstacle_centroids,
+    std::vector<Eigen::Vector3f> &obstacle_sizes,
+    std::vector<Eigen::Quaternionf> &obstacle_orientations,
+    bool enable_verbose = false) {
+  auto startTime = std::chrono::steady_clock::now();
+  obstacle_centroids.clear();
+  obstacle_sizes.clear();
+  obstacle_orientations.clear();
+  for (const auto &obstacle_cloud : obstacle_clouds) {
+    pcl::MomentOfInertiaEstimation<pcl::PointXYZI> feature_extractor;
+    feature_extractor.setInputCloud(obstacle_cloud);
+    feature_extractor.compute();
 
-      pcl::PointXYZI min_point_OBB;
-      pcl::PointXYZI max_point_OBB;
-      pcl::PointXYZI position_OBB;
-      Eigen::Matrix3f rotational_matrix_OBB;
+    pcl::PointXYZI min_point_OBB;
+    pcl::PointXYZI max_point_OBB;
+    pcl::PointXYZI position_OBB;
+    Eigen::Matrix3f rotational_matrix_OBB;
 
-      feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB,
-                               rotational_matrix_OBB);
+    feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB,
+                             rotational_matrix_OBB);
 
+    Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
+    Eigen::Quaternionf quat(rotational_matrix_OBB);
+    obstacle_centroids.push_back(position);
+    obstacle_orientations.push_back(quat);
+    Eigen::Vector3f size;
+    size << (max_point_OBB.x - min_point_OBB.x),
+        (max_point_OBB.y - min_point_OBB.y),
+        (max_point_OBB.z - min_point_OBB.z);
+    obstacle_sizes.push_back(size);
+    // 可视化障碍物框
+    if (false) {
       pcl::visualization::PCLVisualizer::Ptr viewer(
           new pcl::visualization::PCLVisualizer("3D Viewer"));
       viewer->setBackgroundColor(0, 0, 0);
@@ -68,90 +152,91 @@ void ExtractObstacles(
       viewer->initCameraParameters();
       viewer->addPointCloud<pcl::PointXYZI>(obstacle_cloud, "sample cloud");
 
-      Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
-      Eigen::Quaternionf quat(rotational_matrix_OBB);
       // 添加可视化
-      // viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x,
-      //                 max_point_OBB.y - min_point_OBB.y,
-      //                 max_point_OBB.z - min_point_OBB.z, "OBB");
-      // viewer->setShapeRenderingProperties(
-      //     pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
-      //     pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
-      obstacle_centroids.push_back(position);
-      obstacle_orientations.push_back(quat);
-      Eigen::Vector3f size;
-      size << (max_point_OBB.x - min_point_OBB.x) ,(max_point_OBB.y - min_point_OBB.y),(max_point_OBB.z - min_point_OBB.z);
-      obstacle_sizes.push_back(size);
+      viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x,
+                      max_point_OBB.y - min_point_OBB.y,
+                      max_point_OBB.z - min_point_OBB.z, "OBB");
+      viewer->setShapeRenderingProperties(
+          pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
+          pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, "OBB");
     }
+  }
+  auto endTime = std::chrono::steady_clock::now();
+  auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(
+      endTime - startTime);
+  std::cout << "extract obstacles took " << elapsedTime.count() << " milliseconds"
+            << std::endl;
 
-    // debug
-    if(0) {
-        std::cout << obstacle_centroids.size();
-        for(int i=0;i < obstacle_centroids.size();i++) {
-            std::cout << "====" << std::endl;
-            std::cout << obstacle_centroids[i] << std::endl;
-            std::cout << obstacle_sizes[i] << std::endl;
-            // std::cout << obstacle_orientations[i] << std::endl;
-        }
+  // debug
+  if (enable_verbose) {
+    std::cout << obstacle_centroids.size();
+    for (int i = 0; i < obstacle_centroids.size(); i++) {
+      std::cout << "======obstacle[" << i << "]======" << std::endl;
+      std::cout << obstacle_centroids[i] << std::endl;
+      std::cout << obstacle_sizes[i] << std::endl;
+      std::cout << obstacle_orientations[i].matrix() << std::endl;
+      std::cout << "========================" << std::endl;
     }
+  }
 }
 
 /**
  * 可视化障碍物
- * @param viewer PCL可视化器
  * @param obstacle_clouds 障碍物点云集合
  * @param obstacle_centroids 障碍物质心集合
  * @param obstacle_sizes 障碍物尺寸集合
  * @param obstacle_orientations 障碍物旋转集合
  */
-void visualizeObstacles(
-    const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr>& cloudClusters,
-    const std::vector<Eigen::Vector3f>& obstacle_centroids,
-    const std::vector<Eigen::Vector3f>& obstacle_sizes,
-    const std::vector<Eigen::Quaternionf>& obstacle_orientations)
-{
-    // 创建可视化窗口
-    std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
-    viewer->setBackgroundColor(0, 0, 0);
+inline void visualizeObstacles(
+    const std::vector<pcl::PointCloud<pcl::PointXYZI>::Ptr> &cloudClusters,
+    const std::vector<Eigen::Vector3f> &obstacle_centroids,
+    const std::vector<Eigen::Vector3f> &obstacle_sizes,
+    const std::vector<Eigen::Quaternionf> &obstacle_orientations) {
+  // 创建可视化窗口
+  std::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
+      new pcl::visualization::PCLVisualizer("3D Viewer"));
+  viewer->setBackgroundColor(0, 0, 0);
 
-    // 绘制包围盒
-    for (size_t i = 0; i < obstacle_centroids.size(); i++)
-    {
-        
-        std::stringstream cloud_name;
-        cloud_name << "cloud_" << i;
-        viewer->addPointCloud<pcl::PointXYZI>(cloudClusters[i], cloud_name.str());
-        
-        std::stringstream box_name;
-        box_name << "box_" << i;
-        
-      viewer->addCube(obstacle_centroids[i], obstacle_orientations[i], obstacle_sizes[i][0],
-                      obstacle_sizes[i][1],
-                      obstacle_sizes[i][2], box_name.str());
-         // 设置颜色
-        viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 
-                                    0.5,0,0, box_name.str());  
-      viewer->setShapeRenderingProperties(
-          pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
-          pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, box_name.str());
-    }
+  // 绘制包围盒
+  for (size_t i = 0; i < obstacle_centroids.size(); i++) {
 
-    // 启动可视化
-    while (!viewer->wasStopped()) {
-      viewer->spinOnce();
-      // std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
+    std::stringstream cloud_name;
+    cloud_name << "cloud_" << i;
+    viewer->addPointCloud<pcl::PointXYZI>(cloudClusters[i], cloud_name.str());
+
+    std::stringstream box_name;
+    box_name << "box_" << i;
+
+    viewer->addCube(obstacle_centroids[i], obstacle_orientations[i],
+                    obstacle_sizes[i][0], obstacle_sizes[i][1],
+                    obstacle_sizes[i][2], box_name.str());
+    // 设置颜色
+    viewer->setShapeRenderingProperties(
+        pcl::visualization::PCL_VISUALIZER_COLOR, 0.5, 0, 0, box_name.str());
+    viewer->setShapeRenderingProperties(
+        pcl::visualization::PCL_VISUALIZER_REPRESENTATION,
+        pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME,
+        box_name.str());
+  }
+
+  // 启动可视化
+  while (!viewer->wasStopped()) {
+    viewer->spinOnce();
+    // std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
 }
 
-
-// 计算旋转矩阵  
-Eigen::Matrix4d TransforMatrix(Eigen::Vector3d translation,double roll,double pitch,double yaw) {
-    Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())  
-                         * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())  
-                         * Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());  
-    Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity(); // 初始化为单位矩阵  
-    transformation_matrix.block<3, 3>(0, 0) = q.toRotationMatrix(); // 设置旋转矩阵部分  
-    transformation_matrix.block<3, 1>(0, 3) = translation; // 设置平移向量部分  
-    std::cout << "旋转矩阵：" << std::endl << transformation_matrix << std::endl;  
-    return transformation_matrix;
+// 计算旋转矩阵
+inline Eigen::Matrix4d TransforMatrix(Eigen::Vector3d translation, double roll,
+                                      double pitch, double yaw) {
+  Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
+                         Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
+                         Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
+  Eigen::Matrix4d transformation_matrix =
+      Eigen::Matrix4d::Identity(); // 初始化为单位矩阵
+  transformation_matrix.block<3, 3>(0, 0) =
+      q.toRotationMatrix(); // 设置旋转矩阵部分
+  transformation_matrix.block<3, 1>(0, 3) = translation; // 设置平移向量部分
+  std::cout << "旋转矩阵：" << std::endl << transformation_matrix << std::endl;
+  return transformation_matrix;
 }

yaml_config/yaml_config.yaml

@@ -1,6 +1,19 @@
 # 基础ros设置
-sub_cloud_topic : "abc"
-pub_cloud_topic : "efg"
+sub_cloud_topic : "/livox/lidar_192_168_20_105"
+pub_cloud_ground_topic : "cloud_ground"
+pub_cloud_noground_topic : "cloud_noground"
+obstacles_topic : "obstacles_topic"
+# 激光雷达到车体坐标系变换
+convert:
+  transform:
+    x: 0.3643
+    y: 0.2078
+    z: 1.21
+  # 角度，代码里会转成弧度
+  rotation :
+    roll: -179.5
+    pitch: 0.5
+    yaw: 360
 # 降采样大小
 filterRes : 0.05
 # 设置roi区域
@@ -25,10 +38,13 @@ ransac:
   distanceThreshold : 0.1
 # 设置聚类参数
 cluster:
-  clusterTolerance : 0.2
+  clusterTolerance : 0.15
   minsize : 30
-  maxsize : 1000
+  maxsize : 2000
 # 可视化分割地面点云
 view:
   segment : false
+# 打印日志
+log:
+  enable_obstacles: false