#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

class PointCloudToLaserScan
{
public:
    PointCloudToLaserScan()
    {
        // 创建订阅者和发布者
        pointcloud_sub_ = nh_.subscribe("/velodyne_points", 1, &PointCloudToLaserScan::pointCloudCallback, this);
        laser_scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>("/scan", 1);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber pointcloud_sub_;
    ros::Publisher laser_scan_pub_;

    void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
    {
        // 转换 PointCloud2 消息到 pcl::PointCloud
        pcl::PointCloud<pcl::PointXYZ> cloud;
        pcl::fromROSMsg(*cloud_msg, cloud);

        // 创建 LaserScan 消息
        sensor_msgs::LaserScan laser_scan;
        laser_scan.header = cloud_msg->header; // 保持相同的时间戳和坐标系
        laser_scan.angle_min = -M_PI / 2; // 设置最小角度
        laser_scan.angle_max = M_PI / 2;   // 设置最大角度
        laser_scan.angle_increment = M_PI / 180; // 每个激光束之间的角度增量
        laser_scan.range_min = 0.0; // 最小范围
        laser_scan.range_max = 100.0; // 最大范围

        // 计算 LaserScan 数据
        int num_readings = static_cast<int>((laser_scan.angle_max - laser_scan.angle_min) / laser_scan.angle_increment);
        laser_scan.ranges.resize(num_readings);
        
        // 遍历点云，将其转换为激光扫描范围
        for (int i = 0; i < num_readings; ++i)
        {
            float angle = laser_scan.angle_min + i * laser_scan.angle_increment;
            // 找到与当前角度最接近的点
            float min_range = laser_scan.range_max;
            for (const auto& point : cloud.points)
            {
                float point_angle = atan2(point.y, point.x);
                if (fabs(point_angle - angle) < laser_scan.angle_increment / 2)
                {
                    float range = sqrt(point.x * point.x + point.y * point.y);
                    if (range < min_range)
                    {
                        min_range = range;
                    }
                }
            }
            laser_scan.ranges[i] = min_range;
        }

        // 发布 LaserScan 消息
        laser_scan_pub_.publish(laser_scan);
    }
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "pointcloud_to_laserscan");
    PointCloudToLaserScan pcl_to_laser;
    ros::spin();
    return 0;
}