ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

converting velodyne packets to pointcloud

asked 2017-05-04 05:01:49 -0600

shashank gravatar image

updated 2017-05-12 07:19:16 -0600

Hii, I have velodyne packets and its config file . I want to convert it to point cloud in a C++ program. Can anyone provide me API to do this .

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
1

answered 2017-05-12 03:12:11 -0600

shashank gravatar image

here is the api to convert into pcl data structure .After this we can use Pcl_conversion to convert the cloud into sensor_msg::pointcloud2

    #include "velodyne_pointcloud/calibration.h"
    #include "velodyne_msgs/VelodynePacket.h"
    #include "velodyne_msgs/VelodyneScan.h"
    #include "velodyne_pointcloud/rawdata.h"
    #include "velodyne_pointcloud/point_types.h"
    #include "velodyne_pointcloud/VelodyneConfigConfig.h"
    #include "velodyne_pointcloud/point_types.h"
    #include  "velodyne_pointcloud/calibration.h"
    #include <pcl_conversions/pcl_conversions.h>

void save_3d_lidar(velodyne_msgs::VelodyneScan::ConstPtr v_scan,nav_msgs::Odometry::ConstPtr gps_pose)
{



    pcl::PointCloud<pcl::PointXYZI> cloud_3d;
    pcl::PointXYZI point;
    velodyne_rawdata::RawData data;

    cloud_3d.clear();

    for(int i=0;i<v_scan->packets.size();i++)
    {
        pcl::PointCloud<velodyne_pointcloud::PointXYZIR> v_point_cloud;


        data.setParameters(0.9,130.0,0.0,6.28);
        data.setupOffline("file adresss",130.0,0.9);
        data.unpack(v_scan->packets[i],v_point_cloud);

        for(int j=0;j<v_point_cloud.size();j++)
        {

            point.x=v_point_cloud.points[j].x;

            point.y=v_point_cloud.points[j].y;

            point.z=v_point_cloud.points[j].z;

            point.intensity=v_point_cloud.points[j].intensity;
            cloud_3d.push_back(point);
        }
    }
}
edit flag offensive delete link more
2

answered 2017-05-04 09:07:32 -0600

joq gravatar image

The velodyne_pointcloud package does what you seem to need.

edit flag offensive delete link more

Comments

Not exactly...I do not want to run a node for that.I am looking for a library to do it.Basically I need a function in which I give velodyne scan as input and it gives me point cloud as oputput

shashank gravatar image shashank  ( 2017-05-04 09:28:09 -0600 )edit
1

It's not exactly an advertised interface, but I suppose you could use the RawData class: http://docs.ros.org/indigo/api/velody...

joq gravatar image joq  ( 2017-06-12 10:04:06 -0600 )edit

Question Tools

3 followers

Stats

Asked: 2017-05-04 05:01:49 -0600

Seen: 6,030 times

Last updated: May 12 '17