Building point cloud using input from position, orientation and laser scan data

asked 2011-12-16 00:19:19 -0500

alfa_80
1051 ●123 ●137 ●150

updated 2011-12-17 04:28:06 -0500

I am willing to build a point cloud taking input from position and orientation (pose message) data gathered over time along with laser scan message. In achieving this, of course I need to publish a topic of type PointCloud, but, then, the processing procedures, I'm not really sure.

The stored pose data looks like below:

Here is a loop of the data gathered reading some values row by row..

{btQuaternion q(pcl::deg2rad(data_set[t][8]), pcl::deg2rad(data_set[t][7]),
pcl::deg2rad(data_set[t][6]));

        pose3D.position.x =  data_set[t][3];
        pose3D.position.y = data_set[t][4];
        pose3D.position.z = data_set[t][5];

        pose3D.orientation.x = q.x();
        pose3D.orientation.y = q.y();
        pose3D.orientation.z = q.z();
        pose3D.orientation.w = q.w();
}

Could someone provide me any code snippet or example that does the functionality.

answered 2011-12-16 01:01:14 -0500

DimitriProsser

11163 ●79 ●116 ●169

updated 2011-12-16 06:30:11 -0500

ROS provides the laser_geometry package to do just this. Give that a shot. It will make your life much easier.

Edit: Here's some code to publish your odom information. You would publish this at a certain interval in your odometry node, and laser_geometry would be able to read it.

static tf::TransformBroadcaster tfb;
tf::Transform xform;
xform.setOrigin(tf::Vector3(pose3D.position.x, pose3D.position.y, pose3D.position.z));
tf::Quaternion qt(pose3D.orientation.x, pose3D.orientation.y, pose3D.orientation.z, pose3D.orientation.w);
xform.setRotation(qt);
tfb.sendTransform(tf::StampedTransform(xform, ros::Time::now(), "base_frame", "target_frame"));

For more information, check the tf tutorials.