How to feed Point Cloud from a file into a grid_map

edit

Hello, I started working on a project that turns 3d point clouds from a file into grid_map. After successfully having set all my project in CMake I was trying to prepare a small example. I am already able to publish a grid_map from this tutorial as it is possible to see from the print screen below:

Also from this source it seems to be possible to feed a grid_map with some point clouds that I have in a file on my Desktop. However I am finding this process a little bit difficult mostly because I am still not confident with grid_map as I started working with it recently. Any insight on how to solve this problem will be welcome.

Below I am putting the code I am using to try to feed grid_map with a point cloud file I have on my Desktop:

#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;
}

void Point_CLoud_Reader::writeToGridMap()
{
    // .....
}


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 ...