Hello, I just started using the Point Cloud Library. I followed some initial tutorial [here](http://pointclouds.org/documentation/tutorials/reading_pcd.php) 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` 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](http://pointclouds.org/documentation/tutorials/pcd_file_format.php) 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
#include
#include
#include "../include/cloud.h"
#include
#include
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
#include
#include
#include
#include
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::Ptr cloud(new pcl::PointCloud);
if(pcl::io::loadPCDFile (filename, *cloud) == -1) // load point cloud file
{
PCL_ERROR("Could not read the file");
return;
}
std::cout<<"Loaded"<width * cloud->height<<"data points from /home/to/Desktop/file.pcd with the following fields: "<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.
↧