Changing from tf::Transform to tf::StampedTransform
Could someone try to change any part of my code in order to use tf::StampedTransform. I've been trying to just replace it in place of tf::Transform directly, it doesn't work. I should have been missing something I think. This is needed because I cannot see the base_link movement with respect to the world. I think the tf is not correctly published with the below code.
sensor_msgs::PointCloud2 cloud; //Class variable
// In a callback function
{
sensor_msgs::PointCloud2 c_tmp;
static tf::TransformBroadcaster tfb;
tf::Transform xform;
xform.setOrigin(tf::Vector3(pose3D_LDD.pose.position.x, pose3D_LDD.pose.position.y, pose3D_LDD.pose.position.z));
xform.setRotation(tf::Quaternion(pose3D_LDD.pose.orientation.x, pose3D_LDD.pose.orientation.y, pose3D_LDD.pose.orientation.z, pose3D_LDD.pose.orientation.w));
tfb.sendTransform(tf::StampedTransform(xform, ros::Time(pose3D_LDD.header.stamp), "world", "base_link"));
tfListener_.waitForTransform("world", "base_link", previous_scan_.header.stamp, ros::Duration(1.0));
try
{
projector_.transformLaserScanToPointCloud("laser", previous_scan_, c_tmp, tfListener_);
if (!initialized_cloud_)
{
cloud = c_tmp;
initialized_cloud_ = true;
}
else
{
pcl::concatenatePointCloud(cloud, c_tmp, cloud);
ROS_INFO("From c_tmp: Got cloud with %u of width and %u of height", c_tmp.width, c_tmp.height);
ROS_INFO("From cloud: Got cloud with %u of width and %u of height", cloud.width, cloud.height);
}
}
Thanks in advance.