ROS订阅TF消息转换成pose发布

参考：
http://www.ncnynl.com/archives/201702/1311.html

void collect::LaserPoseSubThread()
{
tf::TransformListener listener;
tf::StampedTransform transform;
geometry_msgs::PoseWithCovarianceStamped laser_pose_msg;
while(true)
{
try
{
listener.waitForTransform("/map", "/laser",ros::Time(0), ros::Duration(10.0));
listener.lookupTransform("/map", "/laser",ros::Time(0), transform);
/*
//debug
std::cout<<std::endl<<"pose:"<<std::endl
<<"orig_x:"<<transform.getOrigin().x()<<std::endl
<<"orig_y:"<<transform.getOrigin().y()<<std::endl
<<"orig_z:"<<transform.getOrigin().z()<<std::endl
<<"rota_x:"<<transform.getRotation().getX()<<std::endl
<<"rota_y:"<<transform.getRotation().getY()<<std::endl
<<"rota_z:"<<transform.getRotation().getZ()<<std::endl
<<"rota_w:"<<transform.getRotation().getW()<<std::endl;
*/
laser_pose_msg.header.stamp = transform.stamp_;
laser_pose_msg.pose.pose.position.x = transform.getOrigin().x();
laser_pose_msg.pose.pose.position.y = transform.getOrigin().y();
laser_pose_msg.pose.pose.position.z = transform.getOrigin().z();
laser_pose_msg.pose.pose.orientation.x = transform.getRotation().getX();
laser_pose_msg.pose.pose.orientation.y = transform.getRotation().getY();
laser_pose_msg.pose.pose.orientation.z = transform.getRotation().getZ();
laser_pose_msg.pose.pose.orientation.w = transform.getRotation().getW();
laser_pose_pub_.publish(laser_pose_msg);
ros::Duration(0.1).sleep();

}
catch(tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
}
}
}