转：ROS-TF的使用（常用功能）
https://blog.csdn.net/u010876294/article/details/75004903
一、TF中的数据格式：
这些数据格式全都是class
头文件：#include
（1）tf::Vector3
tf::Vector3 point;
默认构造一个对象：tf::Vector3（float x, float y, float z）
对于tf::Vector3对象还有set与get方法。
1)举例：构造一个xyz分别为0,0,0的对象 tf::Vector3(0,0,0)
2)举例：使用geometry_msgs::PoseStampedPtr pose构造一个对象
tf::Vector3(pose->pose.position.x,pose->pose.position.y,pose->pose.position.z)
（2）tf::Quaternion
tf::Quaternion q;
1) 举例：使用geometry_msgs::PoseStampedPtr pose构造一个对象
tf::quaternionMsgToTF(pose->pose.orientation,q); //把geomsg形式的四元数转化为tf形式，得到q
2) q.setRPY(Rtheta,Ptheta,Ytheta);//根据已经知道的欧拉角进行设置q，分别是child_frame绕着frame坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)
3)tf_q = tf::createQuaternionFromYaw( theta ); theta是绕z轴旋转角,其他两个轴旋转角为0;
如要生成ros_q,可以直接使用 :ros_q = tf::createQuaternionMsgFromYaw(theta);
4) q = tf::Quaternion(0,0,0,1);//直接通过参数设置q，0,0,0,1代表没有任何旋转
拓展：把tf四元数转化为geomsg四元数
geometry_msg::Quaternion msg_q;
tf::quaternionTFToMsg(tf_q,msg_q);
(3)tf::Transform
tf::Transform transform
1) transform = tf::Transform( tf::Quaternion(0,0,0,1) , tf::Vector3(0,0,0) ); //使用参数初始化，得到transform
2) transform. setRotation( tf_q );
transform.setOrigin( tf_point ); //通过set方法得到transform
(4)tf::StampedTransform
tf::StampedTransform stamped_transform
stamped_transform = tf::Transform( transform, ros::Time::now(), "frame", "child_frame"); //使用参数初始化得到
二、tf::TransformBroadcaster
发布TF
头文件：#include< tf / transform_broadcaster.h >
tf::TransformBroadcaster broadcaster; //定义发布器
broadcaster.sendTransform( stamped_transform ) ; //发布一个tf，stamped_transform是一个tf::StampedTransform类型的数据。
三、tf::TransformListener
头文件：#include< tf / transform_listener.h>
tf::TransformListener listener //定义监听器
（1）lookuoTransform(); //获取tf
-
tf::StampedTransform stamped_transform;
//定义存放变换关系的变量
-
try
-
{
-
//得到child_frame坐标系原点，在frame坐标系下的坐标与旋转角度
-
listener.lookupTransform(
"frame",
"child_frame",ros::Time(
0), stamped_transform);
-
}
-
catch (tf::TransformException &ex)
-
{
-
ROS_ERROR(
"%s",ex.what());
-
ros::Duration(
1.0).sleep();
-
continue;
-
}
获取到stamped_transform后可以使用这些信息
-
transform
.getOrigin()
.x()
-
transform
.getOrigin()
.y()
-
-
transform
.getRotation()
.getW();
-
transform
.getRotation()
.getX();
-
transform
.getRotation()
.getY();
-
transform
.getRotation()
.getZ();
（2）transformPoint()
在实际应用中需要将一个坐标系下的点转化到另一个坐标系下
-
//将child_frame坐标系下的Point1，转化到frame坐标系下，存储在point2中
-
geometry_msgs::PointStamped point1;
-
point1.header.stamp=ros::Time();
-
point1.header.frame_id=
"child_frame";
-
point1.point.x=
1;
-
point1.point.y=
2;
-
point1.point.z=
3;
-
geometry_msgs::PointStamped point2;
-
try
-
{
-
listener.transformPoint(
"frame",point1,point2);
-
}
-
catch (tf::TransformException &ex)
-
{
-
ROS_ERROR(
"%s",ex.what());
-
ros::Duration(
1.0).sleep();
-
}
四、四元数转欧拉角
-
tf::Quaternion tf_quat;
-
tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_quat);
-
double roll,pitch,yaw;
-
tf::Matrix3x3(tf_quat).getRPY(roll,pitch,yaw);
-
-
好像可以直接 tf::getYaw(tf_quat);
-
或者 tf::getYaw(amcl_pose.pose.pose.orientation);
-
//括号里面可以为ROS四元数或者tf四元数
-
-
tf::createQuaternionMsgFromYaw(yaw);
-
tf::getYaw(msg->pose.pose.orientation);
五、已知tf::Transform tf 和其中一个坐标系下的 pose 可以将其转化到另一个坐标系下
-
geometry_msgs::Pose transformPose(geometry_msgs::Pose &pose, tf::Transform &tf)
-
{
-
tf::Pose tf_pose;
-
tf::poseMsgToTF(pose,tf_pose);
-
-
tf_pose = tf * tf_pose;
-
-
geometry_msgs::Pose ros_pose;
-
tf::poseTFToMsg(tf_pose, ros_pose);
-
-
return ros_pose;
-
}