tf2 transform error: Lookup would require extrapolation into the past
Hi, I am currently trying to simulate a quadcopter in gazebo using mavros and px4 sitl. In this simulation, the quadcopter will identify an aruco marker, then attempt to center itself with that marker. To do so, I needed to transform the marker frame (on the ground) to the frame of the quadcopter itself (map). I have checked to see if all the frames I am working with are from the same tf but when I run the code, I get the error pictured below.
Using ROS kinetic, on ubuntu 16.04.
tf tree
these are two separate trees.
- local_origin]-->[local_origin_ned]-->[fcu_frd]-->[fcu]
- [map]-->[base_link]-->[marker_frame] (This is the tf I am working with)
Code
#include <ros/ros.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_ros/buffer_interface.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
geometry_msgs::PoseStamped poseIn;
geometry_msgs::PoseStamped poseOut;
bool received;
ros::Duration timeout;
void callback(const geometry_msgs::PoseStamped& msg){
poseIn = msg;
received= true;
}
int main(int argc, char** argv){
ros::init(argc, argv, "listener_test");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe("aruco_single/pose", 100, &callback);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0); //rate 10
sleep(1);
while (nh.ok()){
try{
if(received){poseOut = tfBuffer.transform(poseIn, "map", ros::Duration(0.0));}
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
Error
[ WARN] [1560275941.431970740, 1406.020000000]: Lookup would require extrapolation into the past. Requested time 1405.716000000 but the earliest data is at time 1560275940.551247808, when looking up transform from frame [base_link] to frame [map]
hmm I have it enabled in my gazebo launch file as shown, however it doesn't seem to be doing any good. From what I read it shouldn't have to be enabled more than once correct?
Going to try and see what happens if I enable it in as many launch files as I can.
ok so, it seems to be working when I enable it in multiple launch files. The only problem now is that the same error occurs but it requires extrapolation into the future instead of past,
Okay we're very close now. If you look at the times, the tf buffer is less than 0.02 seconds behind. You're currently calling transform with no timeout (i.e.
Duration(0.0)
) if you change this toDuration(0.1)
then this should start working. This will give the transform call enough time to wait for the most recent TF data to arrive allowing it to successfully transform the pose.Awesome everything seems to be functioning now! Thank you very much for sticking with me through this, I have learned a lot. I really appreciated the help.
You're welcome. Glad you got it working.
hello encountered the same problem and i don't where to change the tf buffer.