pcl_ros::transformPointCloud shows Lookup would require extrapolation into the past
I was trying to transform point cloud from camera frame to base_link frame using pcl_ros::transformPointCloud. However, the TF always show the error of Lookup would require extrapolation into the past. I checked carefully to make sure I have everything setup correctly but can't figure out why I have this error. Could you show me what I did wrong? Here is my 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){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input,*cloud);
cloud_direction_y(cloud);
cloud_direction_z(cloud);
cloud_stat_remove(cloud);
rail_plane(cloud, cloud_projected);
}
void rail_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr input, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected){
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
*cloud = *input;
pcl::ModelCoefficients::Ptr floor_coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr floor_indices(new pcl::PointIndices());
pcl::SACSegmentation<pcl::PointXYZ> floor_finder;
floor_finder.setOptimizeCoefficients(true);
floor_finder.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
floor_finder.setMethodType(pcl::SAC_RANSAC);
floor_finder.setMaxIterations(500);
floor_finder.setAxis(Eigen::Vector3f(0,1,0));
floor_finder.setDistanceThreshold(0.07);
floor_finder.setEpsAngle(MYDEG2RAD(5));
floor_finder.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(*cloud));
floor_finder.segment(*floor_indices, *floor_coefficients);
if (floor_indices->indices.size() > 3)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr floor_points(new pcl::PointCloud<pcl::PointXYZ>);
pcl::ExtractIndices<pcl::PointXYZ> extractor;
extractor.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >(*cloud));
extractor.setIndices(floor_indices);
extractor.filter(*floor_points);
extractor.setNegative(true);
extractor.filter(*cloud);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(floor_points);
proj.setModelCoefficients(floor_coefficients);
proj.filter(*cloud_projected);
floor_points->header.frame_id = input->header.frame_id;
floor_points->header.stamp = input->header.stamp;
sensor_msgs::PointCloud2 msg;
msg.header.stamp = ros::Time().now();
pcl::toROSMsg(*cloud_projected,msg);
rail_plane_pub.publish(msg);
}
}
void fit_railsCB(const sensor_msgs::PointCloud2ConstPtr& input_from_camera_frame){
try{
cam_bl.waitForTransform("base_link","pico_flexx_optical_frame",ros::Time(0),ros::Duration(5));
cam_bl.lookupTransform("base_link","pico_flexx_optical_frame",ros::Time(0),cam_bl_tf);
}
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);
}
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;
}
And the error message:
[ERROR] [1518908418.439292642, 1518732053.305808327]: Lookup would require extrapolation into the past. Requested time 1518732053.118709000 but the earliest data is at time 1518732053.201569502, when looking up transform from frame [pico_flexx_optical_frame] to frame [base_link]
A static tf between base_link -> pico_flexx_link is called in a launch file. pico_flexx_link -> pico_flexx_optical_frame is provided by the camera's driver.