点云数据建图的相关问题

Question

#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <laser_geometry/laser_geometry.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/conversions.h>
#include <tf/transform_listener.h>
#include <iostream>
#include <pcl_ros/point_cloud.h>
#include <vector>

class My_Filter {
public:
    My_Filter();

private:
    void scanCallback_2(const sensor_msgs::LaserScan::ConstPtr& scan);
    void pclCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud);
    void publishAllPoints(const ros::TimerEvent& event);

    ros::NodeHandle node_;
    ros::Subscriber scan_sub_;
    ros::Subscriber pclCloud_sub_;
    ros::Publisher point_cloud_publisher_;
    laser_geometry::LaserProjection projector_;
    tf::TransformListener tfListener_;

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> all_points_; // 使用数组形式记录点云数据
    ros::Timer timer_;
};

My_Filter::My_Filter() {
    scan_sub_ = node_.subscribe<sensor_msgs::LaserScan>("/scan", 100, &My_Filter::scanCallback_2, this);
    point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2>("/cloud2", 100, false);
    tfListener_.setExtrapolationLimit(ros::Duration(0.1));
    pclCloud_sub_ = node_.subscribe<pcl::PointCloud<pcl::PointXYZ>>("/cloud2", 10, &My_Filter::pclCloudCallback, this);
    timer_ = node_.createTimer(ros::Duration(0.1), &My_Filter::publishAllPoints, this);
}

void My_Filter::scanCallback_2(const sensor_msgs::LaserScan::ConstPtr& scan) {
    sensor_msgs::PointCloud2 cloud;
    projector_.transformLaserScanToPointCloud("odom_combined", *scan, cloud, tfListener_);
    pcl::PointCloud<pcl::PointXYZ>::Ptr rawCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(cloud, *rawCloud);
    
    // 将当前点云添加到数组中
    all_points_.push_back(rawCloud);

    for (size_t i = 0; i < rawCloud->points.size(); i++) {
        std::cout << rawCloud->points[i].x << "\t" << rawCloud->points[i].y << "\t" << rawCloud->points[i].z << std::endl;
    }

    point_cloud_publisher_.publish(cloud);
}

void My_Filter::pclCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud(new pcl::PointCloud<pcl::PointXYZ>(*cloud));
    all_points_.push_back(newCloud);
    
    for (size_t i = 0; i < cloud->points.size(); i++) {
        std::cout << "direct_trans: " << cloud->points[i].x << "\t" << cloud->points[i].y << "\t" << cloud->points[i].z << std::endl;
    }
}

void My_Filter::publishAllPoints(const ros::TimerEvent& event) {
    sensor_msgs::PointCloud2 output;
    pcl::PointCloud<pcl::PointXYZ>::Ptr combinedCloud(new pcl::PointCloud<pcl::PointXYZ>);

    // 将所有点云合并到一个新的点云中
    for (const auto& cloud : all_points_) {
        *combinedCloud += *cloud;
    }

    pcl::toROSMsg(*combinedCloud, output);
    output.header.frame_id = "odom_combined";
    output.header.stamp = ros::Time::now();
    point_cloud_publisher_.publish(output);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "my_filter_node");
    My_Filter my_filter;
    ros::spin();
    return 0;
}

这段代码功能只是能够定时发布每一刻的点云数据，不能将之前的点云数据保存
我的目的是通过点云数据来构建点云地图
如何修改这段代码，能够保存点云数据从发布到结束时的所有数据，并将所有数据实时的显示到rviz里

Answer 1

以下回复参考：皆我百晓生、券券喵儿等免费微信小程序作答:

您的代码已经在收集点云数据方面做了很好的工作，只是需要将点云数据持久化并显示到rviz中。要实现您的目标，需要对代码进行以下修改：

1. 保存点云数据：您可以使用一个数据结构（如std::deque）来保存所有的点云数据，这样即使ROS的回调也是异步的，也能确保数据的完整性。每次有新的点云数据，将其添加到这个数据结构中。
2. 实时显示到rviz：使用ROS的sensor_msgs::PointCloud2消息来发布合并后的点云数据，确保rviz可以实时显示。您已经在代码中实现了这一点。
3. 持久化点云数据：除了实时显示，您可能还需要将点云数据保存到文件中。可以使用PCL的IO功能来实现这一点。

以下是修改后的代码片段：

#include <deque> // 用于保存点云数据的容器

class My_Filter {
public:
    My_Filter();

private:
    void scanCallback_2(const sensor_msgs::LaserScan::ConstPtr& scan);
    void pclCloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud);
    void publishAllPoints(const ros::TimerEvent& event);
    void saveAllPoints(const ros::TimerEvent& event); // 新增函数，用于保存点云数据到文件

    // ... 其他成员变量不变 ...

    std::deque<pcl::PointCloud<pcl::PointXYZ>::Ptr> all_points_; // 使用deque保存点云数据
    std::string map_filename_; // 保存地图文件的名称
};

// ... 其他函数不变 ...

void My_Filter::saveAllPoints(const ros::TimerEvent& event) {
    // 合并所有点云到一个云中
    pcl::PointCloud<pcl::PointXYZ> combinedCloud;
    for (const auto& cloud : all_points_) {
        combinedCloud.points.insert(combinedCloud.points.end(), cloud->points.begin(), cloud->points.end());
        combinedCloud.width = combinedCloud.points.size(); // 更新宽度属性，根据需要可能还需要更新其他属性如height等
    }
    all_points_.clear(); // 清空已合并的点云数据

    // 保存地图到文件
    pcl::io::savePCDFileASCII(map_filename_, combinedCloud); // 使用PCL的IO功能保存点云到文件
}

此外，您需要在某个地方设置map_filename_变量来指定保存地图的文件名。例如，每次保存地图时，可以使用时间戳作为文件名的一部分来确保不会覆盖旧的地图文件。还可以根据需要修改保存点云数据的频率。此外，为了能在rviz中更好地可视化地图，您可以考虑为点云数据设置颜色或其他属性。这样，您可以实时地在rviz中查看构建的地图，并将构建的地图保存到文件中。