Как передать облако точек из файла в grid_map в ROS

Question

Я начал работать над проектом, который превращает 3d point clouds из файла в grid_map. После успешной установки всего моего проекта в CMake я пытался подготовить небольшой пример. Я уже могу опубликовать grid_map из этого учебника , как это можно увидеть на экране печати ниже:

Также из этого источника представляется возможным скормить grid_map некоторыми point clouds, которые есть у меня в файле на рабочем столе. Однако я нахожу этот процесс немного сложным в основном потому, что я все еще не уверен в grid_map, так как я начал работать с ним недавно.

Ниже я помещаю код, который я использую, чтобы попытаться передать grid_map файлом point cloud, который у меня есть на рабочем столе:

#include <ros/ros.h>
#include <grid_map_ros/grid_map_ros.hpp>
#include <Eigen/Eigen>
#include <grid_map_msgs/GridMap.h>
#include <string>
#include <cstring>
#include <cmath>
#include <iostream>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

using namespace grid_map;

class Point_CLoud_Reader
{
public:
    Point_CLoud_Reader();
    void pointCloudCallback(const sensor_msgs::PointCloudConstPtr& msgIn);
    void readPCloud();
    void writeToGridMap();
    pcl::PointCloud<pcl::PointXYZ> cloud;

private:
    ros::NodeHandle _node;
    ros::Publisher pCloudPub;
    ros::Subscriber pCloudSub;
    std::string _pointTopic;
};


Point_CLoud_Reader::Point_CLoud_Reader()
{
    _node.param<std::string>("pointcloud_topic", _pointTopic, "detections");
    ROS_INFO("%s subscribing to topic %s ", ros::this_node::getName().c_str(), _pointTopic.c_str());
    pCloudPub = _node.advertise<sensor_msgs::PointCloud>("/point_cloud", 100, &Point_CLoud_Reader::pointCloudCallback, this);
}

void Point_CLoud_Reader::pointCloudCallback(const sensor_msgs::PointCloudConstPtr &msgIn)
{
    sensor_msgs::PointCloud msgPointCloud = *msgIn;
    //msgPointCloud.points = cloud; // <-- Error Here
    pCloudPub.publish(msgPointCloud);
}

void Point_CLoud_Reader::readPCloud()
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ> ("/home/to/Desktop/wigglesbank_20cm.pcd", *cloud) == -1) // load point cloud file
    {
        PCL_ERROR("Could not read the file /home/to/Desktop/wigglesbank_20cm.pcd \n");
        return;
    }
    std::cout<<"Loaded"<<cloud->width * cloud->height
             <<"data points from /home/to/Desktop/wigglesbank_20cm.pcd with the following fields: "
             <<std::endl;

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

int main(int argc, char** argv)
{
        // initialize node and publisher
    ros::init(argc, argv, "grid_map_test");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<grid_map_msgs::GridMap>("grid_map", 1, true);
    ros::Publisher pCloudPub= nh.advertise<sensor_msgs::PointCloud>("point_cloud", 1, true);
    pcl::PointCloud<pcl::PointXYZ> cloud;

    // create grid map
    GridMap map({"elevation"});
    map.setFrameId("map");
    map.setGeometry(Length(1.2, 2.0), 0.03);
    ROS_INFO("Created map with size %f x %f m (%i x %i cells).",
             map.getLength().x(), map.getLength().y(),
             map.getSize()(0), map.getSize()(1));

    // work with grid-map in a loop
    ros::Rate rate(30.0);
    while (nh.ok()) {
        // add data to grid-map and point cloud from file just read
        ros::Time time = ros::Time::now();
        for(GridMapIterator it(map); !it.isPastEnd(); ++it)
            for(int i = 0; i < cloud.points.size(); i++)
            {
                Point_CLoud_Reader readCloud;
                readCloud.readPCloud();   // <-- Not reading point clouds
                Position position;
                map.getPosition(*it, position);
                map.at("elevation", *it) = -0.04 + 0.2 * std::tan(3.0 * time.toSec() + 5.0 * position.y()) * position.x();
            }

        // publish a grid map and point cloud from file just read
        map.setTimestamp(time.toNSec());
        grid_map_msgs::GridMap msg;
        sensor_msgs::PointCloud cloud;
        GridMapRosConverter::toMessage(map, msg);
        //GridMapRosConverter::toPointCloud(msg,cloud); <-- Error Here
        pub.publish(msg);
        pCloudPub.publish(cloud);
        ROS_INFO_THROTTLE(1.0, "Grid map (timestamp %f) published.", msg.info.header.stamp.toSec());
        // wait for next cycle
        rate.sleep();
    }
        return 0;
}

Спасибо, что указали в правильном направлении и пролили свет на этот вопрос