#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/point_cloud_conversion.h>

class LaserScanToPointCloud {
public:
    LaserScanToPointCloud() {
        // 初始化订阅者和发布者
        scan_sub_ = nh_.subscribe("/scan", 10, &LaserScanToPointCloud::scanCallback, this);
        cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/filtered_points", 10);
    }

private:
    void scanCallback(const sensor_msgs::LaserScan::ConstPtr& laser_scan) {
        // sensor_msgs::LaserScan scan = *laser_scan;
        // for(int i = 0; i < scan.ranges.size(); i++){
        //     if(scan.intensities[i] < 100)
        //     {
        //         scan.ranges[i] = 0;
        //         scan.intensities[i] = 0;
        //     }
        // }
        sensor_msgs::PointCloud2 cloud;
        laser_proj_.projectLaser(*laser_scan, cloud);  // 转换 LaserScan 到 PointCloud2

        // 发布转换后的点云
        cloud_pub_.publish(cloud);
        // std::cout << " cloud.header.frame_id = "<< cloud.header.frame_id << std::endl;

        // sensor_msgs::PointCloud cloud;
        // cloud.header.frame_id = "laser";
        // for(int i = 0; i < laser_scan->ranges.size(); i++){
        //     if(laser_scan->intensities[i] > 100){
        //         geometry_msgs::Point32 p;
                
        //         float angle = i * laser_scan->angle_increment + laser_scan->angle_min;
        //         p.x = laser_scan->ranges[i] * cos(angle);
        //         p.y = laser_scan->ranges[i] * sin(angle);
        //         cloud.points.push_back(p);
                
        //     }
        // }

        
        // sensor_msgs::PointCloud2 cloud2;
        // cloud2.header.stamp = ros::Time::now();
        // cloud2.header.frame_id = "laser";
        // sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2);  // 转换 LaserScan 到 PointCloud2

        // // 发布转换后的点云
        // cloud_pub_.publish(cloud2);
    }

    ros::NodeHandle nh_;
    ros::Subscriber scan_sub_;
    ros::Publisher cloud_pub_;
    laser_geometry::LaserProjection laser_proj_;  // 激光投影对象
};

int main(int argc, char** argv) {
    std::cout << "launch laser_scan_to_point_cloud node" << std::endl;
    ros::init(argc, argv, "laser_scan_to_point_cloud");
    LaserScanToPointCloud converter;

    ros::spin();  // 进入循环等待回调
    return 0;
}
