Not being able to build a point cloud out of pose and laser messages
I have been trying for a few weeks to build a point cloud out of pose and laser data that I have, however, it is still not working until now. The code snippet looks like below:
void PointCloudBuilder::computePointCloud()
{
sensor_msgs::PointCloud cloud;
pose3D_LD2.header.stamp = ros::Time::now();
pose3D_LD2.header.frame_id = "pose3D_LD2";
pose3D_LD2.pose.position.x = previous_poseLD_.pose.position.x;
pose3D_LD2.pose.position.y = previous_poseLD_.pose.position.y ;
pose3D_LD2.pose.position.z = previous_poseLD_.pose.position.z ;
pose3D_LD2.pose.orientation.x = previous_poseLD_.pose.orientation.x;
pose3D_LD2.pose.orientation.y = previous_poseLD_.pose.orientation.y;
pose3D_LD2.pose.orientation.z = previous_poseLD_.pose.orientation.z;
pose3D_LD2.pose.orientation.w = previous_poseLD_.pose.orientation.w;
static tf::TransformBroadcaster tfb;
tf::Transform xform;
xform.setOrigin(tf::Vector3(previous_poseLD_.pose.position.x, previous_poseLD_.pose.position.y, previous_poseLD_.pose.position.z));
tf::Quaternion qt(previous_poseLD_.pose.orientation.x, previous_poseLD_.pose.orientation.y, previous_poseLD_.pose.orientation.z, previous_poseLD_.pose.orientation.w);
xform.setRotation(qt);
tfb.sendTransform(tf::StampedTransform(xform, ros::Time::now(), "base_frame", "target_frame"));
projector_.transformLaserScanToPointCloud("base_link", previous_scan_, cloud, tfListener_);
// Do something with cloud.
pose3D_usinglocalizationdata_pub2.publish(pose3D_LD2);
point_cloud_publisher_.publish(cloud);
}
When I issue a command of rostopic echo -c /cloud, it returns nothing there. Also from the terminal, I got an error like below:
terminate called after throwing an instance of 'tf::ExtrapolationException'
what(): Lookup would require extrapolation at time 1324837730.494437690, but only time 1324837730.469437690 is in the buffer, when looking up transform from frame [/laser] to frame [/base_link]
[pointcloud_builder_node-5] process has died [pid 29991, exit code -6].
log files: /home/shah/.ros/log/4995859a-2f26-11e1-84b5-c42c03199b29 /pointcloud_builder_node-5*.log
Am I missing something or the order is not correct? Hope somebody can guide me on this.
EDIT(After adding waitForTransform, need somebody to check the use of it):
tf::StampedTransform transform;
//static tf::TransformBroadcaster tfb;
tf::Transform xform;
xform.setOrigin(tf::Vector3(previous_poseLD_.pose.position.x, previous_poseLD_.pose.position.y, previous_poseLD_.pose.position.z));
tf::Quaternion qt(previous_poseLD_.pose.orientation.x, previous_poseLD_.pose.orientation.y, previous_poseLD_.pose.orientation.z, previous_poseLD_.pose.orientation.w);
xform.setRotation(qt);
//tfb.sendTransform(tf::StampedTransform(xform, ros::Time::now(), "base_frame", "target_frame"));
/*try{
tfListener_.waitForTransform("/base_link", "/laser", ros::Time(0), ros::Duration(3.0));
tfListener_.lookupTransform("/base_link", "/laser", ros::Time(0), transform);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}*/
projector_.transformLaserScanToPointCloud("base_link", previous_scan_, cloud, xform);
// Do something with cloud.
pose3D_usinglocalizationdata_pub2.publish(pose3D_LD2);
point_cloud_publisher_.publish(cloud);