tf::transformBroadcaster shows error of Lookup into the past, best practice for setting up broadcaster and listener
Follow up with my previous question #q283013, currently I have another problem with tf broadcaster with same error of lookup into the past. I am really confused now and I hope that you can help me understand a proper way to set up tf broadcaster/listener. I followed an example here #q90246 and fixed my listener. Here is the code:
#include <...my stuffs..>
class cloudHandler{
public:
cloudHandler():
{
main_sub = nh.subscribe("pico_flexx/points",1,&cloudHandler::mainCB,this);
rail_plane_pub = nh.advertise<sensor_msgs::PointCloud2>("rail_plane",1);
fit_rails_sub = nh.subscribe("rail_plane",1,&cloudHandler::fit_railsCB,this);
}
void mainCB(const sensor_msgs::PointCloud2ConstPtr& input){
<...my stuff...>
}
void rail_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected){
<...my stff...>
}
void fit_railsCB(const sensor_msgs::PointCloud2ConstPtr& input_from_camera_frame){
try{
cam_bl.waitForTransform("base_link",input_from_camera_frame->header.frame_id,input_from_camera_frame->header.stamp,ros::Duration(2));
}
catch(tf::TransformException &ex){
ROS_WARN("%s",ex.what());
};
sensor_msgs::PointCloud2 input_in_bl;
pcl_ros::transformPointCloud("base_link",*input_from_camera_frame,input_in_bl,cam_bl); // this is ok to transform now
<...processing my stuff...>
/*Now I tried to published my result*/
tf::Vector3 rail_origin(p.x,p.y,p.z);
static tf::TransformBroadcaster rail_br;
tf::Transform rail_tf_;
rail_tf_.setOrigin(rail_origin);
rail_tf_.setRotation(q);
rail_br.sendTransform(tf::StampedTransform(rail_tf_,ros::Time::now(),"base_link","rail")); /*Can't send broadcast here, the node keeps dieing and restarting which cause a broken tf*/
geometry_msgs::PoseStamped r_p2;
r_p2.header.stamp = ros::Time::now();
r_p2.header.frame_id = "rail";
r_p2.pose.position.x = p.x;
r_p2.pose.position.y = p.y;
r_p2.pose.position.z = p.z;
r_p2.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_rail);
rail_pose_pub2.publish(r_p2);/*Can't even publish this message*/
}
private:
tf::TransformListener cam_bl;
ros::NodeHandle nh;
ros::Subscriber main_sub, fit_rails_sub;
ros::Publisher rail_pose_pub;
};
int main(int argc, char **argv){
ros::init(argc, argv, "pico_rails_node");
cloudHandler handler;
ros::spin();
return 0;
}
I have no idea why I can't send my tf broadcast. I believe it is proper to stamp my broadcast with ros::Time::now() since it requires sometime to process all my stuff. I also tried to stamp my broadcaster with the time stamp of the incoming message but the error still happened.
[ERROR] [1518989417.344710428, 1518732057.962978755]: Lookup would require extrapolation into the past. Requested time 1518732055.923351000 but the earliest data is at time 1518732056.005952182, when looking up transform from frame [pico_flexx_optical_frame] to frame [base_link]
Obviously, I have missed something about tf here. I understand that:
a tf listener requires some time to cache up all the transforms, so in callback, the first few calls should just return;
if we can look up transform, it means that the tf tree is set up properly, then a broadcaster can broadcast anytime with correct transformations between frame_ids. It is logical to stamp this broadcaster with a current time.
So my question is :
1) Could you show me what I misunderstand and/or miss here?
2) Is there a proper way set up the tf to avoid this situation? Example for my class, I put every publishers and subscribers in class constructor, and all callbacks are for ...
Have you gone through the network setup on the wiki? You may have issues with the synchronization between clocks.
Yes, I did. I did set up ntp so that the robot's NUC is always synced with the current time. I recorded my data with rosbag and work with it on my laptop. I did set use_sim_time true before playing my bag and launching my node.
We're going to need some more info here: what does "keeps dying" mean? A
SEGFAULT
, some other error? As this is CPP, I would try to get a GDB backtrace.Also: any reason to not make
rail_br
a member variable?Finally: you're
catch(..)
ing thetf::TransformException
infit_railsCB(..)
, but then just printing the msg. IfwaitForTransform(..)
throws an exception, continuing with the rest of the callback is pointless, so you should perhaps add areturn
there or something similar.Finally2:
that depends: if the frame you attempt to broadcast is the result of processing the incoming cloud -- which was captured at a specific time, then reusing the stamp ..
.. from the cloud that was the input of your process makes more sense to me: how are 'downstream' consumers ever going to be able to correlate your TF frame with the state of the world (ie: at the time that the cloud was captured)? If
processing my stuff
takes a (hypothetical) 5 mins, ....
rail_tf_
will be broadcast based on 5 min old data. Without the stamp from the cloud that was processed, that will be impossible to detect.exactly. So editting your original question would have been appropriate.
I've merged your answer into your original question.