Ask Your Question
0

LaserScan from bag causing Error: "Lookup would require extrapolation into the past."

asked 2018-03-21 21:20:48 -0600

jpde.lopes gravatar image

updated 2018-03-22 14:22:33 -0600

Hello,

I am trying to convert a LaserScan message from a bag. On my header file, I have the following:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include "sensor_msgs/LaserScan.h"
#include "sensor_msgs/PointCloud.h"
#include <laser_geometry/laser_geometry.h>

class laserScan2PC   {
public:

ros::NodeHandle nh_;
ros::Subscriber scan_sub_;
ros::Publisher cloud_pub_;
tf::TransformListener listener;
tf::StampedTransform transform;
laser_geometry::LaserProjection projector_;
sensor_msgs::PointCloud2 cloud;
laserScan2PC(ros::NodeHandle nh_);

void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in);
};

And on the .cpp file:

#include "../include/PrecisionAgriculture/laserScan2PC.h"

laserScan2PC ::laserScan2PC(ros::NodeHandle nh) : nh_(nh){
// http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20(C%2B%2B)
scan_sub_  = nh.subscribe("/base_scan/scan", 1000, &laserScan2PC::scanCallback, this);
cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/pt_cloud", 1);

}

void laserScan2PC :: scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan_in)
{

    // http://wiki.ros.org/laser_geometry
    if(!listener.waitForTransform(scan_in->header.frame_id, "/base_link",scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment),ros::Duration(2.0)))
    {
         ROS_ERROR("Unable to transform");
    }
    else
    {
        projector_.transformLaserScanToPointCloud("/base_link", *scan_in, cloud, listener);
    }
    cloud_pub_.publish(cloud);
}

The problem is that after having the following error messages (caused by the LookupTransform):

[ERROR] [1521684287.419099514]: Unable to transform
[ERROR] [1521684289.429183124]: Unable to transform
(...)
[ERROR] [1521684303.469321561]: Unable to transform
[ERROR] [1521684305.470800803]: Unable to transform

I get the following error:

terminate called after throwing an instance of 'tf2::ExtrapolationException'
  what():  Lookup would require extrapolation into the past.  Requested time 1357578703.109095188 but the earliest data is at time 1357578703.124125818, when looking up transform from frame [base_scan_link] to frame [base_link]
[laserScan2PC-2] process has died [pid 7674, exit code -6, (...)

Which is caused by the projector_.

I have tried to use ros::Time(0) or ros::Time::now() instead of the scan_in->header.stamp.

I think this is related to the fact that the stamp of the laser scan is not in agreement with the transform stamp, but I couldn't find a solution so far.

Finally, I am using the commands below before I play the bag:

rosparam use_sim_time_true
rosbag play --clock <nameOfTheBag>

Any hints on how to solve the error? Thank you in advance!

EDIT:

I have tried to use try/catch to avoid the exception but the problem remains unsolved, only avoiding the crashing of the program. Something like this:

ros::Time time;

try {
    time = scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment);

    if(!listener.waitForTransform("/base_link", "/servo_link", time, ros::Duration(1.0)))
    {
        ROS_ERROR("Unable to transform");
        return;
    }
    projector_.transformLaserScanToPointCloud("/base_link", *scan_in, cloud, listener);
    ROS_INFO("Transform Retrieved!");
}
catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
    ros::Duration(1.0).sleep();
}

Which causes the output depicted below:

[ERROR] [1521729232.549585422]: Unable to transform
(...)
[ERROR] [1521729243.626700137]: Unable to transform
[ERROR] [1521729244.632651396]: Unable to transform
[ERROR] [1521729244.633163741]: Lookup would require extrapolation into the past.  Requested time 1357578703.149084276 but the earliest data is at time 1357578703.164612891, when looking up transform from frame [base_scan_link] to frame [base_link]
[ INFO] [1521729245.633830180]: Transform Retrieved!
[ INFO] [1521729245.634297009]: Transform Retrieved!
(...)
[ INFO] [1521729245.669169912]: Transform ...
(more)
edit retag flag offensive close merge delete

Comments

Could you show your try&catch block?

tuandl gravatar imagetuandl ( 2018-03-22 04:33:37 -0600 )edit

Yes. I have edited the question.

jpde.lopes gravatar imagejpde.lopes ( 2018-03-22 08:44:51 -0600 )edit

What is the reason for the time stamp offset?

time = scan_in->header.stamp + ros::Duration().fromSec(scan_in->ranges.size()*scan_in->time_increment)
lucasw gravatar imagelucasw ( 2018-03-22 11:48:38 -0600 )edit
1

It is explained here: laser_geometry

It is because the time stamp contains the time of the first measurement, but we need to wait until the last scan measurement.

jpde.lopes gravatar imagejpde.lopes ( 2018-03-22 12:33:43 -0600 )edit

I suppose to do it more robustly there would need to be a lookup to make sure both the first and last message can be transformed, it's possible the first would have fallen out of the buffer. Though it is never perfect because the buffer is even changing perhaps between the wait and transform...

lucasw gravatar imagelucasw ( 2018-03-22 14:01:50 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-03-22 11:44:58 -0600

lucasw gravatar image

Many other questions and #q283013 is similar. Increase the waitForTransform timeout to 4 or 5 seconds or however long it takes to get more point clouds transformed, though the very first ones may never work- the next time you record a bag you'll want to 'pre-roll' it for a second or two to get tf history before the point clouds you want are received.

Because your callback queue size is already very large you shouldn't lose data due to buffer overflow, if you did it would need to be make it even larger.

edit flag offensive delete link more

Comments

Your hint helped a lot. If I decrease the buffer size (I have tried with 10 instead of 1000):

scan_sub_  = nh.subscribe("/base_scan/scan", 10, &laserScan2PC::scanCallback, this);

I am able to visualize the points in RViz, without the "Unable to transform" error appearing on the console.

jpde.lopes gravatar imagejpde.lopes ( 2018-03-22 12:28:17 -0600 )edit

Do you think that is because of the lack the "pre-roll" you just mentioned?

jpde.lopes gravatar imagejpde.lopes ( 2018-03-22 12:29:04 -0600 )edit

Ps: I have seen a lot of similar questions but none the solutions seemed to solve my problem, which is why I've decided to start a new question.

jpde.lopes gravatar imagejpde.lopes ( 2018-03-22 12:42:10 -0600 )edit

I see the issue now- I think your buffer was full of perhaps hundreds of messages that were all waiting a second for the transform and then failing when they needed to have been discarded quickly, and then it may have become full and not allowed very many messages in that would have transformed...

lucasw gravatar imagelucasw ( 2018-03-22 14:03:49 -0600 )edit

... or by the time the good messages got to use the callback they also could have become too old to use? (your stdout doesn't show that though) Is canTransform (with ros::Time(0)?) before waitForTransform with the proper timestamp the proper filter that still allows a large queue_size?

lucasw gravatar imagelucasw ( 2018-03-22 14:05:59 -0600 )edit

Yeah a lot of the questions are just enough different to justify different answers (and the same question differently worded is sometimes valuable for searches), but it is good to link them together.

lucasw gravatar imagelucasw ( 2018-03-22 14:19:45 -0600 )edit

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-03-21 21:20:48 -0600

Seen: 830 times

Last updated: Mar 22 '18