publishing custom pointcloud2 message
hii, I am trying to publish message on topic point cloud 2. The value of x is in double. The value of y is in double. The value of z is in double . The value of intensity in in Uchar.
Can some one suggest a way to do it . I wrote a sample source code . I am getting segmentation fault. is there bug in my code. http://docs.ros.org/indigo/api/sensor...
sensor_msgs::PointCloud2 lidarscan;
sensor_msgs::PointCloud2Modifier modifier(lidarscan);
modifier.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT64,
"y", 1, sensor_msgs::PointField::FLOAT64,
"z", 1, sensor_msgs::PointField::FLOAT64,
"i", 1, sensor_msgs::PointField::INT8);
sensor_msgs::PointCloud2Iterator<double> iter_x(lidarscan, "x");
sensor_msgs::PointCloud2Iterator<double> iter_y(lidarscan, "y");
sensor_msgs::PointCloud2Iterator<double> iter_z(lidarscan, "z");
sensor_msgs::PointCloud2Iterator<char> iter_i(lidarscan, "i");
ros::init(argc, argv, "simdata_publisher");
ros::NodeHandle n;
while (true)
{
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
lidarscan.header.stamp.sec ; //take a look here once
lidarscan.header.frame_id=sOutTwoDLidar.id; //topic name to be published for lidar
lidarscan.width=sOutTwoDLidar.totalScans;
lidarscan.height=1;
lidarscan.point_step=(3*sizeof(double)+sizeof(unsigned char));//calculate the no of bytes in point cloud for each point
lidarscan.row_step=lidarscan.width*lidarscan.point_step;
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
for(int i=0;i<lidarscan.width;++i,++iter_x, ++iter_y, ++iter_z, ++iter_i)
{
*iter_x = sOutTwoDLidar.pointCloud[i].x;//segmentation fault here
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
*iter_y = sOutTwoDLidar.pointCloud[i].y;
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
*iter_z = sOutTwoDLidar.pointCloud[i].z;
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
*iter_i = sOutTwoDLidar.pointCloud[i].intensity;
std::cout<<__LINE__<<" Printing the 2d lidar data "<<std::endl;
}
std::cout<<__LINE__<<" publishing lidar scan to the topic"<<std::endl;
lidar_pub.publish(lidarscan);
std::cout<<__LINE__<<" publishing lidar scan to the topic"<<std::endl;
bEOF = cReadLog.getTwoDLidarLog(sOutTwoDLidar);
dTimestamp[0] = sOutTwoDLidar.timestamp;
thanks