Transform not working with rosbag
I have followed the tutorial to create a tf broadcaster.
Here is the code:
void poseCallback(const sensor_msgs::PointCloud2::ConstPtr& msg){
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, 0);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "xtion_link", "camera_link"));
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "subscribe_and_publish");
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe("/camera/depth/points", 10, &poseCallback);
ros::Rate loop_rate(10);
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
The problem is that the transform does not seem to work, properly atleast. When I run the program and the rosbag file and use rosrun tf view_frames I get a nice tree with all the components in it. However the broadcast time between "xtion_link" and "camera_link" is around 4 seconds while time between other broadcasters is around 0.2 seconds.
If i try to visualize the whole thing in rviz and set xtion_link as fixed frame, it shows that all transforms are OK, except the ones below camera_link. If I set camera_link as fixed frame, it shows that xtion_link is OK and others below it but it says that there is no transform from broadcasters above xtion_link.
I don't know if it's caused by the rosbag file or a programmatic error. Rosbag works perfectly in other aspects.