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 :
Could you show me what I misunderstand and/or miss here?
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 processing incoming messages. So how should I set up tf in a callback so that I can lookup/broadcast my transform whenever I want?
Thank you!
Edit: Hi @gvdhoorn, Thank you for your detailed answer. This is a very long answer so I can't put it in a comment.
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. I included a backtrace from gdb here:
(gdb) bt
#0 0x0000000000428817 in cloudHandler::fit_railsCB(boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&) ()
#1 0x0000000000418acc in boost::detail::function::void_function_obj_invoker1<boost::function<void (boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&)>, void, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> >::invoke(boost::detail::function::function_buffer&, boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const>) ()
#2 0x000000000042ac6c in ros::SubscriptionCallbackHelperT<boost::shared_ptr<sensor_msgs::PointCloud2_<std::allocator<void> > const> const&, void>::call(ros::SubscriptionCallbackHelperCallParams&) ()
#3 0x00007ffff522644d in ros::SubscriptionQueue::call()
() from /opt/ros/kinetic/lib/libroscpp.so
#4 0x00007ffff51cc210 in ros::CallbackQueue::callOneCB(ros::CallbackQueue::TLS*) ()
from /opt/ros/kinetic/lib/libroscpp.so
#5 0x00007ffff51cd683 in ros::CallbackQueue::callAvailable(ros::WallDuration) ()
from /opt/ros/kinetic/lib/libroscpp.so
#6 0x00007ffff522a511 in ros::SingleThreadedSpinner::spin(ros::CallbackQueue*) ()
from /opt/ros/kinetic/lib/libroscpp.so
#7 0x00007ffff520f4cb in ros::spin() ()
from /opt/ros/kinetic/lib/libroscpp.so
#8 0x0000000000412839 in main ()
By including the broadcaster, I have a SEGFAULT like this. But if I don't broadcast anything, my program works just fine.
Also: any reason to not make rail_br a member variable?
No, there isn't. I honestly didn't remember why I didn't make it a member variable. I changed that.
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.
This is my silly mistake, I copied from my previous question without editing it. Yes, in the catch, I included a return so that the first few calls which obviously fails will stop my callback.
Finally2:
I believe it is proper to stamp my broadcast with ros::Time::now() since it requires sometime to process all mys 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 ..
This makes sense to me. I stamped my broadcaster with the incoming message's 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.
This is the reason why I first thought to stamp the output of callback with the current time, however, it is weird to ask the robot to get to the pose 5 mins ago. But this is a question of how to process my raw pointcloud.
Originally posted by tuandl on ROS Answers with karma: 358 on 2018-02-18
Post score: 0
Original comments
Comment by jayess on 2018-02-18:
Have you gone through the network setup on the wiki? You may have issues with the synchronization between clocks.
Comment by tuandl on 2018-02-18:
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.
Comment by gvdhoorn on 2018-02-19:
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?
Comment by gvdhoorn on 2018-02-19:
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.
Comment by gvdhoorn on 2018-02-19:
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 ..
Comment by gvdhoorn on 2018-02-19:
.. 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, ..
Comment by gvdhoorn on 2018-02-19:
.. 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.
Comment by gvdhoorn on 2018-02-19:\
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.
Comment by gvdhoorn on 2018-02-19:
re: your gdb trace: you'll need to (re)build your workspace with CMAKE_BUILD_TYPE set to Debug or RelWithDebInfo. Otherwise the backtrace doesn't really help.
Comment by gvdhoorn on 2018-02-19:\
it is weird to ask the robot to get to the pose 5 mins ago
why exactly? This is precisely one of the use-cases that TF was created for. As long as you have a long enough buffer, asking for a TF frame as it was 30 mins ago is no different than asking for one that is only 2 sec old.
Comment by tuandl on 2018-02-19:\
re: your gdb trace: you'll need to (re)build your workspace with CMAKE_BUILD_TYPE set to Debug
This is another stupid mistake I made. I completely forgot to set DCMAKE_BUILD_TYPE=Debug. After rebuild my package with Debug option, I was able to catch a segfault in <...processing stuff...>,...
Comment by tuandl on 2018-02-19:
...,which is strange that it didn't raise any segfault with DCMAKE_BUILD_TYPE=Release. Anyway, I am trying to fix my algorithm. Thank you for your time :).
Comment by tuandl on 2018-02-19:\
why exactly?
I mean it is weird for my robot to ask for a pose that was too far in the past. But I totally understand the purpose of TF is to keep track of every transformation through time.
Comment by gvdhoorn on 2018-02-20:\
I was able to catch a segfault in <...processing stuff...>
Ok. It would be good to post this as an answer (but please add some more detail / explanation, perhaps the backtrace) and then accept your own answer.
That way it's completely clear that your issue has been resolved - and by yourself.