0
$\begingroup$

Rosanswers logo

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.


Originally posted by tuandl on ROS Answers with karma: 358 on 2018-02-17

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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%20and%20Time%20(C++)


Originally posted by lucasw with karma: 8729 on 2018-02-17

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by tuandl on 2018-02-18:
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.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.