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.
Originally posted by RayROS on ROS Answers with karma: 108 on 2019-06-07
Post score: 0
Original comments
Comment by gvdhoorn on 2019-06-08:
@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.