TF problems with laser scan to point cloud and reference frames

edit

Hello everybody!

Basically I try run several Laserscanner on one robot. The idea is to create a class LaserScanSensor which reads the laserscan data from the topic "scan" and transforms its data to a desired frame , e.g. in the given example the world frame. As the robot is moving, also its frame is constantly changing. So the idea is to create a second class RobotBaseLink that changes the sensor reference frame when receiving new pose. Accordingly for the example the frames are : world->robot->laser whereas the robot frame always changes and the laser is fixed relatively to the robot frame.

The problem is, if I run the nodes, than I always get the exception:

terminate called after throwing an instance of 'tf2::LookupException'
  what():  "world" passed to lookupTransform argument target_frame does not exist.

This is caused in LaserScanSensor, when I try to do the laserscan to cloud transformation: projector.transformLaserScanToPointCloud(tf_cloud_frame_id,*msg, cloud,tf_listener); If I use the actual sensor frame "laser", i don't get the error, but when I use "world" or "robot" as a reference frame to calculate the point cloud. However I defined the world to robot frame transformation in RobotBaseLink and the robot to sensor transformation in the LaserScanSensor constructor. Also I can see the frames, when I check tf in rviz. Do you have any idea how I can resolve this issue? Thank you very much in advance for your help!

class LaserScanSensor{
protected:
    ros::NodeHandle                 node;
    ros::Subscriber                 data_subscriber;
    string                          tf_reference_frame_id;
    string                          tf_sensor_frame_id;
    string                          tf_cloud_frame_id;
    tf::Transform                   tf_sensor_frame;
    tf::TransformListener           tf_listener;        //Listen transformed message
    tf::TransformBroadcaster        tf_broadcaster;//Broadcast frame
    laser_geometry::LaserProjection projector;
    sensor_msgs::PointCloud         cloud;
public:
    LaserScanSensor(
            std::string _name,
            std::string _data_topic_name,
            std::string _sensor_frame_id,
            std::string _robot_frame_id,
            std::string _cloud_frame_id,
            geometry_msgs::PoseStamped _pose_sensor2robotframe
    ){
        tf_reference_frame_id=_robot_frame_id;
        tf_sensor_frame_id   =_sensor_frame_id;
        tf_cloud_frame_id   =_cloud_frame_id;
        //Create data subscriber
        data_subscriber = node.subscribe(node.resolveName(_data_topic_name), 1, &LaserScanSensor::sensordata_callback,this);
        ROS_INFO("Subscriber of topic %s activated",_data_topic_name.c_str());

        //Define coordinate transformation from robot to sensor
        tf_broadcaster;
        ros::Duration(1).sleep();//Sleep before broadcasting
        tf_sensor_frame.setOrigin( tf::Vector3(
                _pose_sensor2robotframe.pose.position.x,
                _pose_sensor2robotframe.pose.position.y,
                _pose_sensor2robotframe.pose.position.z));
        tf::Quaternion q(
                _pose_sensor2robotframe.pose.orientation.x,
                _pose_sensor2robotframe.pose.orientation.y,
                _pose_sensor2robotframe.pose.orientation.z,
                _pose_sensor2robotframe.pose.orientation.w);
        tf_sensor_frame.setRotation(q);
        tf_broadcaster.sendTransform(tf::StampedTransform(
                tf_sensor_frame, ros::Time::now(),tf_reference_frame_id, tf_sensor_frame_id)
        );
    }
    //Read laser data and print it according to the given cloud frame
    virtual void sensordata_callback(const sensor_msgs::LaserScan::ConstPtr & msg){
        projector.transformLaserScanToPointCloud(tf_cloud_frame_id,*msg, cloud,tf_listener);
        printf("Number of Points No: %lu\n",cloud.points.size());
        tf::TransformListener           listener;
        for (int i=0;i<cloud.points.size();i++){
            printf("No:%d: x:%2.2f y:%2.2f z:%2.2f\n",i,
                    cloud.points[i].x,cloud.points[i].y,cloud.points[i].z);
        }
    }
};

//Class to dynamically change the robot reference frame according to a PoseStamped topic

class RobotBaseLink{
protected:
    ros::NodeHandle                 node;
    ros::Subscriber                 pose_subscriber;

    string                          tf_reference_frame_id;
    string                          tf_robot_frame_id;
    tf::Transform                   tf_robot_frame;
    tf::TransformListener           tf_listener;        //Listen transformed message
    tf::TransformBroadcaster        tf_broadcaster;

    geometry_msgs::PoseStamped      pose_sensor2uavframe;
    geometry_msgs ...