Ask Your Question
0

How to read a point cloud (.pcd) file and send it to a rosbag

asked 2019-06-07 18:15:40 -0500

RayROS gravatar image

Hello, I just started using the Point Cloud Library. I followed some initial tutorial here on how to use it. I am trying to send a point cloud file .pcd into a rosbag. I would like the rosbag to receive both the ascii and the binary files. I know that the point cloud library has the useful function pcl::io::loadPCDFile<pcl::PointXYZ> that I implemented on the following example (see below the snippet of code). However I am not sure how to go back to a new line to read the file and make the rosbag understand that. My idea was to create a void nextLine() function that does that. But I stopped because I realized that for doing that I will have to write a .pcd parser from scratch according to the structure of the point cloud file. I am not sure how to move on.

Final goal would be able to read any .pcd file and send them to a rosbag --> rosbag play example.bag

See below the snipped of code I am using:

pointcloud_reader_node.cpp

#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include "../include/cloud.h"
#include <rosbag/bag.h>
#include <memory.h>
using namespace std;

int main()
{
    rosbag::Bag bag;
    bag.open("test.bag", rosbag::bagmode::Write);
    std::string fstring = "/home/to/Desktop/file.pcd";
    Cloud p;
    p.readPCloud(fstring);
//    while())
//    {
//        bag.write("point_cloud/data", p.pCloud2Msg.header.stamp , p.pCloud2Msg);
//    }
    return 0;
}

cloud.h

#include "cloud.h"

#ifndef CLOUD_H
#define CLOUD_H

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <sensor_msgs/PointCloud2.h>
#include <string>

class Cloud
{
public:
    void readPCloud(std::string filename);
    sensor_msgs::PointCloud2 pCloud2Msg;
    void nextLine();

private:
    unsigned int msgPointCloud;
    void packPointCloudMsg();
    std::string path;
};

#endif// CLOUD_H

cloud.cpp

#include "cloud.h"

void Cloud::readPCloud(std::string filename)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if(pcl::io::loadPCDFile<pcl::PointXYZ> (filename, *cloud) == -1) // load point cloud file
    {
        PCL_ERROR("Could not read the file");
        return;
    }
    std::cout<<"Loaded"<<cloud->width * cloud->height
             <<"data points from /home/to/Desktop/file.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 Cloud::nextLine()
{

}


void Cloud::packPointCloudMsg()
{
    ros::Time time = ros::Time::now();
    pCloud2Msg.header.stamp = time;
    pCloud2Msg.header.frame_id = "point_cloud";
    pCloud2Msg.header.seq = msgPointCloud;
    pCloud2Msg.height = 1;
    pCloud2Msg.width = 5;
    pCloud2Msg.is_bigendian = false;
    pCloud2Msg.is_dense = false;
}

Thank you very much for pointing in the right direction or providing some example if anyone has ever had this problem before.

edit retag flag offensive close merge delete

Comments

@RayROS: I have the impression you are misunderstanding how things work / go together right now.

If you could clarify what it is that you're actually trying to do, perhaps we can explain.

Things don't get "send to a rosbag". And:

But I stopped because I realized that for doing that I will have to write a .pcd parser from scratch according to the structure of the point cloud file. I am not sure how to move on.

PCL comes with all sorts of IO infrastructure that can read and write .pcd files. I'm not sure why you believe you'd need to implement that yourself.

gvdhoorn gravatar imagegvdhoorn ( 2019-06-08 04:48:37 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
2

answered 2019-06-08 04:40:48 -0500

Hopefully this will clear up what you need to do to get this working. Firstly rosbags store ros messages, so you cannot save a cloud in pcd format into one. You'll need to publish the point cloud as a sensor_msgs/PointCloud or sensor_msgs/PointCloud2 message and use rosbag record to write the message into a bag file. It is possible to write ROS messages directly to a rosbag but that is probably unnecessarily complicated in your case.

So you need to read the pcd file as you are now, and then publish it as a point cloud message on a topic. Then you will run your node while using rosbag record to save the point cloud topic to a bag file. I'm not sure what you're trying to do with the next line function?

Hope this helps.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2019-06-07 18:15:40 -0500

Seen: 299 times

Last updated: Jun 08