TF problems with laser scan to point cloud and reference frames
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;
}
Asked by JanOr on 2016-06-08 08:24:32 UTC
Comments