Revision history [back]

Looks like the problem is that when you receive a laser scan, its timestamp is later than all the available /tf data. This is a common issue in a distributed system, and is what message filters were designed to address. You should try using a message filter.

In the meantime, to help understand the problem, below is a slightly modified version of the code you pointed to. In this version, I added a sleep to the laser scan callback to allow for /tf data to be received before trying to transform the latest scan. By default, this sleep would block receipt of all data, including /tf; so I created a multithreaded spinner. What you should see if you run this program is that at first transforms will fail as your /tf subscriber hooks up and starts getting data; then they'll start succeeding.

Again, don't use the code here; use message filters instead.

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <laser_geometry/laser_geometry.h>

class My_Filter {
     public:
        My_Filter();
        void scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan);
     private:
        ros::NodeHandle node_;
        laser_geometry::LaserProjection projector_;
        tf::TransformListener tfListener_;

        ros::Publisher point_cloud_publisher_;
        ros::Subscriber scan_sub_;
};

My_Filter::My_Filter(){
        scan_sub_ = node_.subscribe<sensor_msgs::LaserScan> ("/multisense_sl/laser/scan", 100, &My_Filter::scanCallback, this);
        point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2> ("/cloud", 100, false);
        tfListener_.setExtrapolationLimit(ros::Duration(0.1));
}

void My_Filter::scanCallback(const sensor_msgs::LaserScan::ConstPtr& scan){
    // Sleep a little so that we have a better chance of having current /tf data
    // before trying to transfrom this latest /scan.
    ros::Duration(0.1).sleep();
    sensor_msgs::PointCloud2 cloud;
    try
    {
      projector_.transformLaserScanToPointCloud("pelvis", *scan, cloud, tfListener_);
    }
    catch(tf::TransformException& e)
    {
      printf("failed to transform: %s\n", e.what());
      return;
    }
    printf("success\n");
    point_cloud_publisher_.publish(cloud);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "my_filter");

    My_Filter filter;

    // Start up two threads, one to handle /tf messages and the other for /scan
    // messages.
    ros::MultiThreadedSpinner spinner(2);
    spinner.spin();

    return 0;
}