Ask Your Question
0

pcl_ros::transformPointCloud shows Lookup would require extrapolation into the past

asked 2018-02-17 17:36:17 -0500

tuandl gravatar image

updated 2018-02-18 16:16:35 -0500

jayess gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2018-02-17 21:25:46 -0500

lucasw gravatar image

updated 2018-02-17 21:42:49 -0500

You'll get that error until the tf listener gets some history into it, the first few callbacks should just return when it happens instead of continuing on to try the transformPointCloud- except you aren't correctly using waitForTransform to detect the problem.

Put the timestamp of the received PointCloud2 into waitForTransform instead of ros::Time(0). lookupTransform isn't necessary at all unless you are using cam_bl_tf somewhere else in this code that you didn't paste here. The code in #q90246 is a decent example - they are also more correctly using the frame supplied in the incoming message and not a hardcoded one.

http://wiki.ros.org/tf/Tutorials/tf%2...

edit flag offensive delete link more

Comments

Hi Lucasw, Thank you for your quick reply. I followed your suggestion, use time stamp of incoming messages instead of asking for the most current one and the first few callbacks now return when it can't transform point cloud. It works now.

tuandl gravatar imagetuandl ( 2018-02-18 03:14:23 -0500 )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

1 follower

Stats

Asked: 2018-02-17 17:36:17 -0500

Seen: 765 times

Last updated: Feb 18 '18