LaserScan from bag causing Error: "Lookup would require extrapolation into the past."
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 ...
Could you show your try&catch block?
Yes. I have edited the question.
What is the reason for the time stamp offset?
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.
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...