Single-line radar fitting circle and visualization in ROS2

1. PCL point cloud learning

1.1. Introduction to PCL

/*
	PCL（Point Cloud Library）作为我们接触的第一个外部库，可见其重要性。
	PCL里实现了大量的处理点云相关的功能，实现了大量点云相关的通用算法和高效数据结构，
	涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。
*/	

/*
    当我们要对激光雷达的数据进行处理时，我们也需要将ROS获取的雷达数据，
    即 sensor_msgs::mgs::LaserScan转化为PCL点云库可以使用的数据  
    如`pcl::PointCloud<T>`．，这就是需要学习PCL的原因。
*/

1.2. Data conversion method

/*
	通过上图我们可得知数据转换的方法步骤为
		1、首先我们需要订阅激光雷达topic（如/scan）获取到sensor_msgs::LaserScan的message．然后使用ROS提供的	   laser_geometry包将其转化为sensor_msgs::PointCloud2格式的message
		2、将sensor_msgs::PointCloud2的message转换为PCL点云库所需的数据格式。
		这里有两种方式
			方式一：使用ROS提供的pcl_conversions包。
			方式二：直接订阅之前转化的PointCloud2的数据。
		
		这样可以自动完成两种类型的转换		
*/
/****注******/
// scan的数据形式类似极坐标，PointCloud的数据形式类似笛卡尔坐标。这里需要有一个坐标转换。
	X = R * cos(θ)
	Y = R * sin(θ)
  其中，(X, Y) 是笛卡尔坐标系中的点坐标，R 是极坐标中的距离，θ 是极坐标中的角度。

1.3. Use of PCL point cloud

// 使用PCL（Point Cloud Library）库来创建一个点云（PointCloud）对象，并初始化了一个名为cloud的指针，指向这个点云对象。
   pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud(new pcl::PointCloud<pcl::PointXYZ>);
// 创建点云发布者 这里需要注意其中  消息类型需要相同
	rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;
	pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("dzj_scan", 10);

1.4. PassThrough Filtering in PCL

PassThrough Filtering is a basic filtering method in point cloud processing. It can be used to clip the point cloud according to the user-defined axial range and only retain points within the specified range. This is useful for removing points that are not within a specific area or extracting points in a region of interest.

pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud);					 // cloud 是点云数据
pass.setFilterFieldName("z");  				 // 以 Z 轴为例
pass.setFilterLimits(min_limit, max_limit);  // 设置滤波的范围
pass.filter(*cloud);

2. ROS2 performs circle fitting based on pcl point cloud data

2.1 Build workspace

mkdir -p ~/scan_get/src
cd ~/ros2_ws
colcon build
source install/setup.bash

2.2 Build ROS2 dependency package

Remember to enter the scr under workspace scan_get to create a function package.

Because subscribing to messages here requires two functional packages, rclcpp and std_msgs.

cd ~/scan_get/src
//这种方式需要手动添加add_executable()、ament_target_dependencies()、install(TARGETS DESTINATION lib/${PROJECT_NAME}) 内容
//ros2 pkg create my_lidar_listener --build-type ament_cmake --dependencies rclcpp sensor_msgs
//这种方式 --node-name lidar_subscriber 后面加上这句话就不用自己手动添加了。这种方式也不用手动创建cpp文件
ros2 pkg create my_lidar_listener --build-type ament_cmake --dependencies rclcpp sensor_msgs --node-name lidar_subscriber

2.3. Create CPP file

In the src directory of the function package my_lidar_listener, create a new C++ file lidar_listener.cpp.

The complete directory structure of the function package at this time is as follows

\src
    └── my_lidar_listener
        ├── CMakeLists.txt
        ├── include
        │   └── my_lidar_listener
        ├── package.xml
        └── src
            └── lidar_listener.cpp

2.4. Add the required dependencies in poackage.xml to ensure that the package can be compiled normally.

# 在 <depend> 标签中添加包所依赖的其他ROS2软件包。这些依赖项告诉ROS2构建系统，该包需要这些软件包来编译和运行。
  <depend>pcl</depend>
  <depend>pcl_conversions</depend>

2.5. Add ROS2 dependencies and build rules in CmakeList.txt

# 确保通过 find_package() 来查找所需的依赖项，并在构建时包含它们
find_package(PCL REQUIRED)
find_package(pcl_conversions REQUIRED)
# 将依赖项包含到构建中
ament_target_dependencies(lidar_listener
  "rclcpp"
  "sensor_msgs"
  "PCL"
  "pcl_conversions"
)

If the uppercase PCL in the above CmakeList.txt dependency is written as lowercase pcl, the following error will be reported

  CMake Error at CMakeLists.txt:24 (find_package):
  By not providing "Findpcl.cmake" in CMAKE_MODULE_PATH this project has
  asked CMake to find a package configuration file provided by "pcl", but
  CMake did not find one.

2.6. Data types that can be published in ros2->publish();

在ROS2中，publish()函数可以用来发布各种不同类型的ROS2消息。发布函数的参数应该是与发布器声明的消息类型相匹配的消息对象。

sensor_msgs/msg/Image	Message type used for image data, such as camera images.
sensor_msgs/msg/PointCloud2	Message type used for point cloud data.
geometry_msgs/msg/PoseStamped	Message type used to represent pose (position and orientation) information.
nav_msgs/msg/Odometry	Message type used to represent odometer information for a robot or vehicle.
std_msgs/msg/String	Message type used to post string data.
custom ROS 2 messages	You can also create custom ROS 2 message types

发布器的声明应该与你要发布的消息类型匹配。例如，如果你要发布激光扫描数据，你应该创建一个发布器，其消息类型为sensor_msgs/msg/LaserScan，然后使用该发布器的publish()函数来发布激光扫描数据。

The picture below shows a typical error

This requires data type conversion and conversion to msg under ros.

// 将PCL点云转换为PointCloud2消息
sensor_msgs::msg::PointCloud2 cloud_msg;
pcl::toROSMsg(*my_cloud, cloud_msg);
// 设置PointCloud2消息的头信息
cloud_msg.header = msg->header;
// 发布点云数据
pointcloud_publisher_->publish(cloud_msg);

3. Complete code

// 用来获取点云数据
#include <iostream>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

// 用来得到云数据
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>

// 用来 点云分割 
#include <pcl/filters/passthrough.h>

// 用来实现 欧式距离聚类
#include <pcl/segmentation/extract_clusters.h>


// 用来拟合圆圈
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>

// 画圆
#include <math.h>



using std::placeholders::_1;

class My_lidar_listener : public rclcpp::Node
{
public:
  /********构造函数******************/
  My_lidar_listener() : Node("laser_scan_to_pointcloud_converter")
  {     
        RCLCPP_INFO(this->get_logger(), "scan话题订阅者创建完成");
    /********创建订阅者*************************************/
    subscription_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
      "scan", 10, std::bind(&My_lidar_listener::lidarCallback, this, std::placeholders::_1));
    /********创建发布者*************************************/
    pointcloud_publisher_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("point_coud", 10); //加::Ptr>报错
  } 

  /********把雷达得到的 LaserScan数据 转换从成 点云的PointXYZ数据*******************************************/
  pcl::PointCloud<pcl::PointXYZ>::Ptr My_scan_to_PointXYZ(const sensor_msgs::msg::LaserScan::SharedPtr msg, pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud);

  /********定义函数对点云数据进行裁减**********************/  
  pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_clipping(pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,float xlim_min,float xlim_max,float ylim_min,float ylim_max);
  /********定义圆拟合函数，实现对圆的 圆心坐标 半径大小 输出*******************************************/
  void My_circle_fitting(const pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,
                          double &center_x, double &center_y, double &radius);


private:
  void lidarCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg)
  {  
/**************将激光扫描数据转换为点云******************************/
    // 使用PCL（Point Cloud Library）库来创建一个点云（PointCloud）对象，并初始化了一个名为cloud的指针，指向这个点云对象。//创建点云对象：使用PCL库创建一个空的点云对象
   pcl::PointCloud<pcl::PointXYZ>::Ptr m_cloud(new pcl::PointCloud<pcl::PointXYZ>);   
   m_cloud = this->My_scan_to_PointXYZ(msg, m_cloud);

  /// number = m_cloud->points.size(); 这将返回m_cloud中存储的点云点的数量 
  //  x = m_cloud->points.at(1).x;     查看点云中点 1 的 x轴 的坐标/
  // m_cloud->points.push_back()
/********对点云数据进行裁减******************/  
    m_cloud = this->pcl_clipping(m_cloud, -10, 10, -10, 10);
  

/********利于欧式距离进行聚类******************/  
/*
  这里需要根据实际的情况进行参数的调整
  setClusterTolerance：表示两个点之间的最大距离，超过这个距离的点将不会被分到同一个群集中。
  setMinClusterSize  ：表示 最小多少个点云才可以视为一个类的
  setMaxClusterSize  ：表示 在一个点云中最大对少个点才可以视为一个类
*/

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> European_clustering;
    European_clustering.setInputCloud(m_cloud);
    European_clustering.setClusterTolerance(0.025); // 设置聚类的欧式距离阈值
    European_clustering.setMinClusterSize(50);    // 设置最小聚类点数
    European_clustering.setMaxClusterSize(25000);  // 设置最大聚类点数
    // European_clustering.setSearchMethod(tree);

    // 这个类用于存储欧氏聚类算法执行后的聚类结果。可以通过访问 cluster_indices 向量来获取每个簇的点的索引
    std::vector<pcl::PointIndices> cluster_indices;
    European_clustering.extract(cluster_indices);

    // 看一个一共聚成了多少个类
    // std::cout<<"点的个数"<<m_cloud->points.size()<<
    //   "聚成的类的个数"<<cluster_indices.size()<<std::endl;

    // 下面这个操作是滤出一些离散的点 当点聚集的数量少于 setMinClusterSize  这些点就会被剔除
    
    // 创建一个新的点云对象来存储所有的点云簇
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr clustered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    //圆的参数
    double center_x, center_y, radius;

    // 遍历所有的点云簇
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
      // 这个用来拟合圆
      pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointXYZRGB color_point;
      color_point.r = 0; // 红色
      color_point.g = 100;
      color_point.b = 200;

      // 遍历当前点云簇中的所有点索引
      for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)
      {
        // 从原始点云中将 点云簇 添加到新的点云对象中
        pcloud_circle->points.push_back(m_cloud->points[*pit]);

         // 从原始点云中将点添加到新的点云对象中
        color_point.x = m_cloud->points[*pit].x;
        color_point.y = m_cloud->points[*pit].y;
        color_point.z = 0;

        clustered_cloud->points.push_back(color_point);
      }
      // 设置 新点云簇 宽度和高度（如果有需要）
      pcloud_circle->width = pcloud_circle->points.size();
      pcloud_circle->height = 1;  // 因为是无序点云，高度设置为1

      /***********执行圆的拟合**************************************/
      
      this->My_circle_fitting(pcloud_circle,center_x,center_y,radius);      // 对点云簇进行圆形拟合   并判哪个是要识别对象 邦把圆心坐标和半径 通过引用的方式和传出来

      // 把原有的数据簇给清空然后开始储存下一轮的数据
      pcloud_circle->clear();
      
    }

    // 设置新点云的宽度和高度（如果有需要）
    clustered_cloud->width = clustered_cloud->points.size();
    clustered_cloud->height = 1;  // 因为是无序点云，高度设置为1
      
/*********** 画圆圈 **************************************/
      // 将圆上的点添加到PointCloud中，并设置颜色
    for (double angle = 0; angle <= 2 * M_PI; angle += 0.01) {
        pcl::PointXYZRGB point;
        point.x = center_x + radius * cos(angle);
        point.y = center_y + radius * sin(angle);
        point.z = 0.0;
        // 设置每个点的不同颜色
        point.r = 255; // 红色
        point.g = 0;
        point.b = 0;
        clustered_cloud->push_back(point);
    }

/************将PointXYZ点云数据转换为PointCloud2消息类型********并把消息发布出去**********/ 
    sensor_msgs::msg::PointCloud2 cloud_msg;          //这将创建一个PointCloud2类型的ROS 2消息指针cloud_msg，用于存储转换后的点云数据。 
    pcl::toROSMsg(*clustered_cloud, cloud_msg);               // 将ROS消息头转换为PCL消息头    
    cloud_msg.header = msg->header;                   // 设置PointCloud2消息的头信息
    pointcloud_publisher_->publish(cloud_msg);        // 发布点云数据

  }

/************ 一些类的 实例化 *****************/
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr subscription_;
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_publisher_;

};



int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<My_lidar_listener>());
  rclcpp::shutdown();
  return 0;
}



  /********把雷达得到的 LaserScan数据 转换从成 点云的PointXYZ数据*******************************************/
pcl::PointCloud<pcl::PointXYZ>::Ptr My_lidar_listener::My_scan_to_PointXYZ(const sensor_msgs::msg::LaserScan::SharedPtr my_msg, pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud)
{
    // 获取激光扫描的相关信息
    float angle_min = my_msg->angle_min;
    float angle_increment = my_msg->angle_increment;

    for (size_t i = 0; i < my_msg->ranges.size(); ++i)
    {
      float range = my_msg->ranges[i];

      // 计算当前点的角度   //angle_increment 激光扫描角度的增量（弧度） 
      float angle = angle_min + i * angle_increment;  //angle_min：激光扫描的起始角度（弧度）

      // 计算当前点的坐标
      pcl::PointXYZ point;
      point.x = range * std::cos(angle);
      point.y = range * std::sin(angle);
      point.z = 0.0;  // 在这个示例中，将所有点的z坐标设置为0

      // 将点添加到点云中
      my_cloud->push_back(point);
    }

  return my_cloud;

}


  /********定义函数对点云数据进行裁减**********************/  
pcl::PointCloud<pcl::PointXYZ>::Ptr My_lidar_listener::pcl_clipping(pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,float xlim_min,float xlim_max,float ylim_min,float ylim_max)
{
	//创建x轴裁剪对象
    pcl::PassThrough<pcl::PointXYZ> passX;
    passX.setInputCloud(my_cloud);
    passX.setFilterFieldName("x");
    passX.setFilterLimits(xlim_min, xlim_max);  //裁剪保留区域 -8m-8m
    passX.filter(*my_cloud);

    //创建y轴裁剪对象
    pcl::PassThrough<pcl::PointXYZ> passY;
    passY.setInputCloud(my_cloud);
    passY.setFilterFieldName("y");
    passY.setFilterLimits(ylim_min, ylim_max);  //裁剪保留区域 -8m-8m
    passY.filter(*my_cloud);

    return my_cloud;
}

/**********圆形拟合***************************************/
 void My_lidar_listener::My_circle_fitting(const pcl::PointCloud<pcl::PointXYZ>::Ptr my_cloud,
                                          double &center_x, double &center_y, double &radius)
 {
    // 使用体素网格降采样以加快计算（可选）
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(my_cloud);        /******************参数替换为自己的点云数据集********************/
    vg.setLeafSize(0.01, 0.01, 0.01); // 设置体素网格的大小
    vg.filter(*my_cloud);              /******************参数替换为自己的点云数据集合********************/

    // 创建分割对象
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);

    // 设置模型类型（这里使用圆模型）
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_CIRCLE2D); // 或者使用SACMODEL_CIRCLE3D
    seg.setMethodType(pcl::SAC_RANSAC);

    // 设置RANSAC的最大迭代次数和拟合阈值
    seg.setMaxIterations(1000);
    seg.setDistanceThreshold(0.01);

    // 执行分割
    seg.setInputCloud(my_cloud);   /******************参数替换自己的点云数据集********************/
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.size() == 0)
    {
        std::cerr << "Failed to estimate a circle for the given dataset." << std::endl;
        return ;
    }

    float fitting_RMSE = 0;
    float X_tall = 0;
    float Y_tall = 0;

    for(int point_number = 0; point_number < my_cloud->points.size(); point_number++ )
    {
        X_tall = X_tall + std::pow((my_cloud->points[point_number].x - coefficients->values[0]), 2);
        Y_tall = Y_tall + std::pow((my_cloud->points[point_number].y - coefficients->values[1]), 2);
    }
      
     fitting_RMSE = std::sqrt(X_tall + Y_tall) / my_cloud->points.size();
    
    if(coefficients->values[2] * 100 >= 18 && coefficients->values[2] * 100 <= 22 && fitting_RMSE < 0.030)
    {
      // coefficients->values[0] 是圆心x坐标
      // coefficients->values[1] 是圆心y坐标
      // coefficients->values[2] 是圆半径

      // 这里需要加上逻辑判断，判断这个这次拟合出来的 圆 是否符合

      std::cout << "拟合的圆形参数: " <<"圆心X坐标："<< coefficients->values[0] * 100
                << "，  圆心Y坐标："  << coefficients->values[1] * 100
                <<"，  圆的半径："   << coefficients->values[2] * 100<< "cm"
                <<"拟合的误差："      << fitting_RMSE <<std::endl;
            
              
      center_x = coefficients->values[0] ;
      center_y = coefficients->values[1] ;
      radius = coefficients->values[2] ;

    }

 }