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(tfcloudframeid,*msg, cloud,tflistener); 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::PoseStamped      pose_sensor2worldframe;
    laser_geometry::LaserProjection projector;
    sensor_msgs::PointCloud         cloud_sensor_frame;
    sensor_msgs::PointCloud         cloud;
public:
RobotBaseLink(
        std::string _name,
        std::string _pose_topic_name,
        std::string _robot_frame_id,
        std::string _reference_frame_id,
        geometry_msgs::PoseStamped _pose_robot2referenceframe
){
    tf_reference_frame_id=_reference_frame_id;
    tf_robot_frame_id    =_robot_frame_id;

    //Create pose subscriber
    pose_subscriber = node.subscribe(node.resolveName(_pose_topic_name), 1, &RobotBaseLink::sensorpose_callback,this);
    ROS_INFO("Subscriber of topic %s activated",_pose_topic_name.c_str());
    //Define Robot to reference frame
    tf_broadcaster; //Broadcast frame
    ros::Duration(1).sleep();//Sleep before broadcasting
    tf_robot_frame.setOrigin( tf::Vector3(_pose_robot2referenceframe.pose.position.x,_pose_robot2referenceframe.pose.position.y,_pose_robot2referenceframe.pose.position.z));
    tf::Quaternion q(_pose_robot2referenceframe.pose.orientation.x,_pose_robot2referenceframe.pose.orientation.y,_pose_robot2referenceframe.pose.orientation.z,_pose_robot2referenceframe.pose.orientation.w);
    tf_robot_frame.setRotation(q);
    ros::Duration(1).sleep();//Sleep before broadcasting
    tf_broadcaster.sendTransform(tf::StampedTransform(tf_robot_frame, ros::Time::now(),tf_reference_frame_id,tf_robot_frame_id));
}
    //Reading PoseStamped Message and transform the robot_frame to reference frame transformation
    virtual void sensorpose_callback(const geometry_msgs::PoseStamped::ConstPtr & msg){
        tf::poseMsgToTF(msg->pose,tf_robot_frame);
        tf_broadcaster.sendTransform(tf::StampedTransform(tf_robot_frame, msg->header.stamp,tf_reference_frame_id,tf_robot_frame_id));
    }

};

//MAIN

int main(int argc, char **argv)
{
    //Ros Init
    ros::init(argc, argv, "listener");

    geometry_msgs::PoseStamped dummy;
    dummy.pose.position.x=1;
    dummy.pose.position.y=0;
    dummy.pose.position.z=0;
    dummy.pose.orientation.x=0;
    dummy.pose.orientation.y=0;
    dummy.pose.orientation.z=0;
    dummy.pose.orientation.w=1;

    RobotBaseLink(
            "Robot1",
            "pose",
            "robot",//Robot     Frame ID
            "world",//Reference Frame ID
            dummy
    );
    LaserScanSensor a(
            "LASER1",
            "scan",
            "laser",//Sensor    Frame ID
            "robot",//Robot Reference Frame ID
            "world",//Cloud data Reference Frame ID
            dummy
    );

    //Checking for messages
    ros::spin();

    //Exit node
    return 0;
}