Ask Your Question
0

publishing custom pointcloud2 message

asked 2016-02-03 06:04:31 -0500

shashank gravatar image

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2019-06-18 11:27:13 -0500

ChuiV gravatar image

You need to allocate enough space in your new point cloud before you try to put any data into it:

sensor_msgs::PointCloud2Modifier modifier(lidarscan);
// create iterators
modifier.resize(<number_of_points_to_allocate>);
// loop through all your points, incrementing the pc iterators
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

2 followers

Stats

Asked: 2016-02-03 06:04:31 -0500

Seen: 1,022 times

Last updated: Jun 18