3d点云转2d

rviz_MID360.launch会发送sensor_msgs/PointCloud2话题类型的/livox/lidar，是rviz可以显示的点云类型
msg_MID360.launch会发送livox_ros_driver2/CustomMsg话题类型的/livox/lidar，是fast_lio要求输入的数据类型

1.mid360获取3d点云

获取特定方向点云数据

构造结构体用于存储点云数据

class PointCloud {
public:
    // 构造函数
    PointCloud() {}

    // 添加点的方法
    void addPoint(double x, double y, double z) {
        points.push_back({x, y, z});
    }

    // 获取点的数量
    size_t size() const {
        return points.size();
    }

    // 获取所有点的坐标
    std::vector<std::vector<double>> getPoints() const {
        return points;
    }

    // 打印所有点的坐标
    void printPoints() const {
        std::cout << "Points in the PointCloud:" << std::endl;
        int index = 1;
        for (const auto& point : points) {
            std::cout << "x: " << point[0] << "  y: " << point[1] << "  z: " << point[2] << "index:"<<index++<<std::endl;
        }
    }

    // 发布点云数据
    void publishPointCloud(ros::Publisher& pub,std::string frame_id) {
        // 创建 PointCloud2 消息
        sensor_msgs::PointCloud2 msg;
        msg.header.stamp = ros::Time::now();
        msg.header.frame_id = frame_id;
        msg.height = 1;
        msg.width = points.size();
        msg.is_dense = true;

        // 设置字段
        sensor_msgs::PointCloud2Modifier modifier(msg);
        modifier.setPointCloud2FieldsByString(1, "xyz");
        sensor_msgs::PointCloud2Iterator<float> iter_x(msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(msg, "z");

        // 将点的坐标添加到消息中
        for (const auto& point : points) {
            *iter_x = point[0];
            *iter_y = point[1];
            *iter_z = point[2];
            ++iter_x; ++iter_y; ++iter_z;
        }

        // 发布消息
        publisher.publish(msg);
    }

private:
    // 存储点的坐标
    std::vector<std::vector<double>> points;
};

2.3d点云转2d点云

2.1 安装rslidar驱动，参考：https://www.ncnynl.com/archives/201807/2552.html

2.2 编译pointcloud_to_laserscan源码

2.3 假设工作空间为rslidar_ws

cd ~/rslidar_ws/src
git clone https://github.com/ros-perception/pointcloud_to_laserscan
cd ~/rslidar_ws
catkin_make
source ~/.bashrc #假设你已经把工作空间添加到bashrc文件

2.4 新建rslidar.launch

cd ~/rslidar_ws/src/pointcloud_to_laserscan/launch
vim  rslidar.launch

<?xml version="1.0"?>    
<launch>    
    <!-- run pointcloud_to_laserscan node -->
    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
 
        <remap from="cloud_in" to="/rslidar_points"/>
        <remap from="scan" to="/rslidar/scan"/>
        <rosparam>
            # target_frame: rslidar # Leave disabled to output scan in pointcloud frame
            transform_tolerance: 0.01
            min_height: -0.4
            max_height: 1.0
 
            angle_min: -3.1415926 # -M_PI
            angle_max: 3.1415926 # M_PI
            angle_increment: 0.003 # 0.17degree
            scan_time: 0.1
            range_min: 0.2
            range_max: 100
            use_inf: true
            inf_epsilon: 1.0
 
            # Concurrency level, affects number of pointclouds queued for processing and number of threads used
            # 0 : Detect number of cores
            # 1 : Single threaded
            # 2->inf : Parallelism level
            concurrency_level: 1
        </rosparam>
    </node>
</launch>

2.5参数说明

min_height 、max_height #这两个参数用来指定这段z轴高度范围内的输入点云将参与转换，范围外的不参与
angle_min、angle_max #这两个参数用来指定这段yaw角度范围内的输入点云将参与转换，范围外的不参与
angle_increment #输出的2d雷达数据的角分辨率，即相邻扫描点的角度增量
scan_time #扫描时间，即话题的发布周期
range_min、range_max #这两个参数用来指定输出数据的有效距离，即2d雷达的有效测量范围

3.测试

新开终端打开雷达

roslaunch rslidar_pointcloud rs_lidar_16.launch

打开rviz

新开终端，启动转换

roslaunch pointcloud_to_laserscan rslidar.launch

在rviz中勾掉pointcloud2

增加话题，/rslida/scan