1.数据类型

数据类型定义在tf/transform_datatypes.h.里

1.1 基本数据类型(Quaternion, Vector, Point, Pose, Transform)

Type	tf
Quaternion	tf::Quaternion
Vector	tf::Vector3
Point	tf::Point
Pose	tf::Pose
Transform	tf::Transform

1.2 tf::Stamped

tf::Stamped在上面的数据类型(tf::Transform除外)上被模板化，元素有frame_id_和stamp_

template <typename T>
class Stamped : public T{public:ros::Time stamp_;std::string frame_id_;Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //仅用于预分配的默认构造函数Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);void setData(const T& input);
};

1.3 tf::StampedTransform

tf::StampedTransform是tf::Transforms的一个特例，同时要求具有frame_id，stamp 和 child_frame_id。

/** \tf使用的stamp Transform数据类型*/
class StampedTransform : public tf::Transform
{
public:ros::Time stamp_; ///< The timestamp associated with this transform  std::string frame_id_; ///< The frame_id of the coordinate frame in which this transform is defined                                                            std::string child_frame_id_; ///< The frame_id of the coordinate frame this transform defines                                                                   StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };/** \brief Default constructor only to be used for preallocation */StampedTransform() { };/** \brief Set the inherited Traonsform data */void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};};

2. 辅助函数

tf::Quaternion createIdentityQuaternion()//返回一个四元数
tf::Quaternion createQuaternionFromRPY(double roll,double pitch,double yaw)

返回一个由由固定轴横滚角、俯仰角和偏航角组成的tf::Quaternion

geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll,double pitch,double yaw)

返回一个由由固定轴横滚角、俯仰角和偏航角组成的geometry_msgs::Quaternion

3.TF使用方法

当我们使用TF包时，需要编写两个程序，分别用来执行监听TF变换和广播TF变换的功能，我们称它们为TF监听器和TF广播器。

• TF监听器：监听TF变换，接收并缓存系统中发布的所有参考系变换，并从中查询所需要的参考系变换。
• TF广播器：广播TF变换，向系统中广播参考系之间的坐标变换关系。系统中可能会存在多个不同部分的tf变换广播，但每个广播都可以直接将参考系变换关系直接插入tf树中，不需要再进行同步。

4. TransformBroadcaster

此类提供了一种发布坐标系变换信息的简便方法。它将处理所有消息和消息填充。函数原型列出了每条消息所需的所有必要数据。

void tf::TransformBroadcaster::sendTransform(const StampedTransform & transform)

发送StampedTransform，stamped数据结构包括frame_id、time和parent_id。

void tf::TransformBroadcaster::sendTransform(const std::vector< StampedTransform > & transforms)

发送一个vector的StampedTransform，stamped数据结构包括frame_id、time和parent_id。

void tf::TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & transform)

发送一个StampedTransform message，stamped数据结构包括frame_id、time和parent_id。

void tf::TransformBroadcaster::sendTransform(const std::vector< geometry_msgs::TransformStamped > & transforms)

发送一个vector的StampedTransform message，stamped数据结构包括frame_id、time和parent_id。

测试代码

// 创建tf的广播器static tf::TransformBroadcaster br;// 初始化tf数据tf::Transform transform;transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );tf::Quaternion q;q.setRPY(0, 0, msg->theta);transform.setRotation(q);// 广播world与海龟坐标系之间的tf数据br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

运行结果

5. TransformListener

此类继承自Transformer，并自动订阅 ROS transform messages。

lookupTransform

函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。

(1)这个lookupTransform()API，有六个参数：

• 变换从坐标系turtle2
• 到turtle1坐标系
• 在now时间
• 变换结果保存的变量

transformPoint

函数功能：这个在传感器数据的坐标变换中使用的比较多，用来将一个传感器的数据从一个坐标系转换到另外一个坐标系下。

waitForTransform

函数功能：等待变换在tf树中生效
(1) waitForTransform()有四个参数：

• turtle2，父类坐标系
• carrot1，子类坐标系
• rospy.Time()，这个时间的变换，就是变换的时刻
• rospy.Duration(4.0)，等待最长的时间段

(2) waitForTransform() 实际上会阻塞，阻塞时间由第四个参数决定，直到两个坐标系变换开始。（通常是几毫秒）
(3) waitForTransform()常常会调用两次，一开始产生turtle2时候 ，在第一次调用waitForTransform()前，TF可能还没有更新。第一次调用waitForTransform()就会等待，直到/turtle2坐标系开始广播。第二次调用waitForTransform()用Now参数才能获取到值。

transfromPose

函数功能：转换位姿，将某位姿转换到指定坐标系下的位姿表示。
 

测试代码

int main(int argc, char** argv)
{// 初始化ROS节点ros::init(argc, argv, "my_tf_listener");// 创建节点句柄ros::NodeHandle node;// 请求产生turtle2ros::service::waitForService("/spawn");ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");turtlesim::Spawn srv;add_turtle.call(srv);// 创建发布turtle2速度控制指令的发布者ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);// 创建tf的监听器tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){// 获取turtle1与turtle2坐标系之间的tf数据tf::StampedTransform transform;try{listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令geometry_msgs::Twist vel_msg;vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),transform.getOrigin().x());vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +pow(transform.getOrigin().y(), 2));turtle_vel.publish(vel_msg);rate.sleep();}return 0;
};

运行结果
同上

6. Matrix3x3

它是一个3x3的矩阵，有3个Vector3 m_el[3]，(此时使用的是行向量)

定义及赋值
//用四元数构造3x3旋转矩阵
Matrix3x3(const Quaternion& q)Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
//赋值构造函数
Matrix3x3 (const Matrix3x3& other)常用的函数
//四元数转旋转矩阵
void setRotation(const Quaternion& q)
//欧拉角 转旋转矩阵
setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw)
//旋转矩阵转四元数
getRotation(Quaternion& q)
// 旋转矩阵转欧拉角
getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1)
//矩阵转置
Matrix3x3 transpose() const;
//矩阵求逆
Matrix3x3 inverse() const;

还有如下函数操作
运算符重载=，*=

测试代码

inline void printQuaternion(const tf::Quaternion& q, const std::string& info)
{// std::cout << "quaternion: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << std::endl;  // x,y,z,wstd::cout << info << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; // 两种元素访问方式均可
}inline void printMatrix(const tf::Matrix3x3& mat, const std::string info)
{std::cout << info << "-------------------- " << std::endl;std::cout << std::setprecision(4) << mat[0][0] << "\t" << mat[0][1] << "\t" << mat[0][2] << "\n"<< mat[1][0] << "\t" << mat[1][1] << "\t" << mat[1][2] << "\n"<< mat[2][0] << "\t" << mat[2][1] << "\t" << mat[2][2] << "\n\n";
}int main(int argc, char** argv)
{ros::init(argc, argv, "transform_ros");ros::NodeHandle nh;// 1, 旋转矩阵初始化方法：double theta = M_PI / 3.0;double ct = cos(theta), st = sin(theta);double roll = M_PI / 2.0, pitch = M_PI / 3.0, yaw = M_PI / 4.0;tf::Quaternion q(0, 0, 0, 1);  // x,y,z,w;tf::Matrix3x3 mat1(q);  // 从四元数初始化旋转矩阵, 内部通过 setRotation 实现;tf::Matrix3x3 mat2(ct, -st, 0, st, ct, 0, 0, 0, 1);  // 直接初始化;tf::Matrix3x3 mat3(mat2);tf::Matrix3x3 mat4 = mat3;tf::Matrix3x3 mat5;mat5.setRPY(roll, pitch, yaw);  // 欧拉角到旋转矩阵的变换, 内部直接调用 setEulerYPR(yaw, pitch, roll) 函数来实现;printMatrix(mat1, "mat1:");printMatrix(mat2, "mat2:");printMatrix(mat3, "mat3:");printMatrix(mat4, "mat4:");printMatrix(mat5, "mat5:");// 2, 旋转矩阵与其它旋转量间的转换:// rotation matrix --> euler:mat2.getRPY(roll, pitch, yaw, 1);  // 第四个参数 solution_number, 缺省时默认值为1；// 函数内部是直接使用 getEulerYPR(yaw, pitch, roll, solution_number); 方法实现的;std::cout << "rpy from mat2: " << roll << ", " << pitch << ", " << yaw << std::endl;mat5.getEulerYPR(yaw, pitch, roll);std::cout << "rpy from mat5(solution_number not set): " << roll << ", " << pitch << ", " << yaw << std::endl;mat5.getEulerYPR(yaw, pitch, roll, 1);std::cout << "rpy from mat5(solution_number = 1): " << roll << ", " << pitch << ", " << yaw << std::endl;mat5.getEulerYPR(yaw, pitch, roll, 0);  std::cout << "rpy from mat5(solution_number = 0): " << roll << ", " << pitch << ", " << yaw << std::endl;// 可以看到，solution_number 为0和1时得到的结果时不一样的，它设置为默认值1时的结果与原结果是一致的，所以强烈建议使用getRPY函数时，直接缺省第四个参数；// 其中的差别主要在于solution_number = 0时，对计算得到的pitch有：pitch = M_PI - pitch; 然后利用该pitch计算roll和yaw，使得最后的结果不一样；// rotation matrix --> quaternion:mat2.getRotation(q);printQuaternion(q, "quaternion from matrix mat2: ");// quaternion --> rotation matrix:mat1.setRotation(q);printMatrix(mat1, "\nmat from q(from mat2): ");// rotation matrix --> euler:// mat.getRPY(roll, pitch, yaw);// euler --> rotation matrix:// mat.setRPY(roll, pitch, yaw);// 3, 其它操作:tf::Vector3 vec1, vec2, vec3;vec1 = mat1[2];vec2 = mat1.getColumn(1);vec3 = mat2.getRow(0);std::cout << "get column of matrix[2]: " <<  vec1[0] << ", " << vec1[1] << ", " << vec1[2] << std::endl;std::cout << "get column of matrix[1]: " <<  vec2[0] << ", " << vec2[1] << ", " << vec2[2] << std::endl;std::cout << "get row of matrix[0]: " <<  vec3[0] << ", " << vec3[1] << ", " << vec3[2] << std::endl;return 0;
}

运行结果

7. MessageFilter

tf下的消息过滤器。
tf::MessageFilter可以订阅任何的ROS消息，然后将其缓存，直到这些消息可以转换到目标坐标系，然后进行相应的处理（一般在回调函数中处理）。说白了就是消息订阅+坐标转换。实际上，后者继承于前者：

• 定义数据：TransformListener、message_filters::Subscriber、tf::MessageFilter
• 用消息的名称来初始化message_filters::Subscriber
• 用tf、message_filters::Subscriber、目标坐标系来初始化tf::MessageFilter
• 给tf::MessageFilter注册callback。
• 编写callback，并在回调中完成坐标转换。至此完成消息订阅+坐标转换

测试代码

message_filters::Subscriber<std_msgs::UInt32> sub(nh, "my_topic", 1);
sub.registerCallback(myCallback);

8. TimeCache

这个类构建并维护一个带有时间戳的数据列表。并提供查找函数，以根据时间的变化获取数据。

9. TransformException

TF异常
所有在TF里异常继承自tf::TransformException，它继承自std::runtime_error。

异常类型
tf::ConnectivityException
如果由于两个坐标系ID不在同一个连接的树中而无法完成请求，则抛出。

tf::ExtrapolationException
如果请求的坐标系id之间存在连接，但一个或多个变换已过期，则抛出。

tf::InvalidArgument
如果参数无效则抛出。 最常见的情况是非规范化的四元数。

tf::LookupException
如果引用了未发布的坐标系ID，则抛出。
 

测试代码

catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}