从文件夹中读取所有的pcd文件并发布

andlbds

已于 2024-07-22 10:34:42 修改

阅读量59

点赞数 2

文章标签：自动驾驶人工智能机器学习

于 2024-07-22 10:27:22 首次发布

本文链接：https://blog.csdn.net/weixin_60822173/article/details/140602870

版权

代码非常简单，不多说了直接上代码

#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <pcl_ros/point_cloud.h> // 注意pcl_ros的不同
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <random>
#include <glob.h>
#include <fstream>

class PointCloudPublisher
{
public:
    PointCloudPublisher()
    {
        // 初始化节点句柄
        nh_ = new ros::NodeHandle("~");

        msg_ = sensor_msgs::PointCloud2();

        filenames = std::vector<std::string>(100);

        pcd_dir = "/home/and/work/ld_ws/src/extrinsic_calibration/src/pcd/";

        getFiles(pcd_dir);

        point_cloud_publisher_ = nh_->advertise<sensor_msgs::PointCloud2>("pointcloud_publisher", 10);

        // 设置定时发布点云
        timer_ = nh_->createTimer(ros::Duration(5.0 / 1.0), &PointCloudPublisher::timerCallback, this); // 每1秒发布一次
    }

    void timerCallback(const ros::TimerEvent &)
    {
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
        for (auto &i : filenames)
        {
            ROS_INFO("读取点云文件：%s",  i.c_str());
            pcl::PCDReader reader;
            reader.read(i.c_str(), *cloud);
            if (!cloud->points.empty())
            {
                // 更新消息时间戳
                pcl::toROSMsg(*cloud, msg_);
                msg_.header.stamp = ros::Time::now();
                ROS_INFO("发布点云");
                msg_.header.frame_id = "rslidar";
                point_cloud_publisher_.publish(msg_);
            }
            else
            {
                ROS_INFO("点云为空");
            }
        }
    }

    void getFiles(std::string path)
    {
        DIR *pDir;
        struct dirent *ptr;
        if (!(pDir = opendir(path.c_str())))
        {
            cout << "Folder doesn't Exist!" << endl;
            return;
        }
        while ((ptr = readdir(pDir)) != 0)
        {
            if (strcmp(ptr->d_name, ".") != 0 && strcmp(ptr->d_name, "..") != 0)
            {
                filenames.push_back(path + "/" + ptr->d_name);
            }
            ROS_INFO("文件名为：%s",  ptr->d_name);
        }
        closedir(pDir);
    }

private:
    ros::NodeHandle *nh_;
    ros::Publisher point_cloud_publisher_;
    sensor_msgs::PointCloud2 msg_;
    ros::Timer timer_;
    std::vector<std::string> filenames;
    std::string pcd_dir;
};

int main(int argc, char **argv)
{
    setlocale(LC_CTYPE, "zh_CN.utf8");
    ros::init(argc, argv, "pointcloud_publisher");
    PointCloudPublisher gpcp;
    ros::spin();
    return 0;
}