Ask Your Question
0

tf::transformBroadcaster shows error of Lookup into the past, best practice for setting up broadcaster and listener

asked 2018-02-18 16:08:41 -0500

tuandl gravatar image

updated 2018-02-19 03:46:38 -0500

gvdhoorn gravatar image

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:

  1. a tf listener requires some time to cache up all the transforms, so in callback, the first few calls should just return;

  2. 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 ... (more)

edit retag flag offensive close merge delete

Comments

Have you gone through the network setup on the wiki? You may have issues with the synchronization between clocks.

jayess gravatar imagejayess ( 2018-02-18 16:22:21 -0500 )edit

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.

tuandl gravatar imagetuandl ( 2018-02-18 16:48:15 -0500 )edit
1

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?

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 01:41:07 -0500 )edit

Finally: you're catch(..)ing the tf::TransformException in fit_railsCB(..), but then just printing the msg. If waitForTransform(..) throws an exception, continuing with the rest of the callback is pointless, so you should perhaps add a return there or something similar.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 01:44:09 -0500 )edit

Finally2:

I believe it is proper to stamp my broadcast with ros::Time::now() since it requires sometime to process all my stuff

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 ..

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 01:45:36 -0500 )edit
1

.. 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, ..

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 01:47:19 -0500 )edit

.. 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.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 01:47:57 -0500 )edit

Thank you for your detailed answer. This is a very long answer so I can't put it in a comment.

exactly. So editting your original question would have been appropriate.

I've merged your answer into your original question.

gvdhoorn gravatar imagegvdhoorn ( 2018-02-19 03:42:02 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-02-20 07:45:36 -0500

tuandl gravatar image

re: your gdb trace: you'll need to (re)build your workspace with CMAKE_BUILD_TYPE set to Debug

I was able to catch a segfault in <...process my stuff...> which seems to cause a segfault in a broadcaster object.

I am still curious about my error though: if I build my package in Release mode, the segfault caused by my algorithm wouldn't crash my node without asking a broadcaster to broadcast my result. It would only crash when I ask a broadcaster to broadcast the result.

But this is a different question. Thank you for your help @gvdhoorn. Please close this question.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-02-18 16:08:41 -0500

Seen: 91 times

Last updated: Feb 20 '18