ROS-TF的使用（常用功能）

tf 使用_人非人1991的博客-CSDN博客

一、TF中的数据格式：

这些数据格式全都是class

头文件：#include <tf/transform_datatypes.h>基本上可以包含所有的tf数据类型

（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轴)

已知q，获取roll,pitch,yaw

tf::Matrix3x3(q).getRPY(roll,pitch,yaw);

3)tf_q = tf::createQuaternionFromYaw( theta ); theta是绕z轴旋转角,其他两个轴旋转角为0;

tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);

如要生成ros_q,可以直接使用 :ros_q = tf::createQuaternionMsgFromYaw(theta);

ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);

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::Transform作为父类继承而来的

tf::StampedTransform stamped_transform

stamped_transform = tf::StampedTransform( transform, ros::Time::now(), "frame", "child_frame"); //使用参数初始化得到

可以直接获取stamped_transform.getRotation(), stamped_transform.getOrigin();

二、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_q;
tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_q);
double roll,pitch,yaw;
tf::Matrix3x3(tf_q).getRPY(roll,pitch,yaw);//四元数转yaw角
tf::getYaw(tf_q);
tf::getYaw(ros_q);/********欧拉角转四元数*****/
ros_q = tf::createQuaternionMsgFromYaw(yaw);
rt_q = f::createQuaternionFromYaw(yaw);
ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);
tf_q.setRPY(roll,pitch,yaw);void GetXYZWFromRPY(double roll, double pitch, double yaw, double& x, double& y, double& z, double& w)
{roll /= 2;pitch /= 2;yaw /= 2;x = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);y = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);z = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);w = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
}

五、已知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;
}