ROS中的坐标变换

参考图书1

简介

坐标系通过三维空间中的一个点来定义，这个点p被称为坐标原点，从坐标原点延伸出来三个向量，n，t，b，分别对应局部坐标系中的x，y，z。向量轴是标准化的，并且符合右手定则：b=n×tb=n×t。

三个方向的坐标轴变换可以堆在一起，用3×33×3的变换矩阵R表示坐标轴的旋转变换R=⎡⎣⎢⎢⎢nxnynztxtytzbxbybz⎤⎦⎥⎥⎥R=[nxtxbxnytybynztzbz]

将原点向量包含进去，用4×44×4的其次变换矩阵表示为：

T=⎡⎣⎢⎢⎢⎢nxnynz0txtytz0bxbybz0pxpypz1⎤⎦⎥⎥⎥⎥T=[nxtxbxpxnytybypynztzbzpz0001]

以上矩阵的计算是可逆的，并且逆矩阵的计算是很方便的。其次变换矩阵之间的计算按照如下形式进行：ATC=ATBBTCATC=ATBBTC表示坐标系{C}相对于坐标系{A}的变换。

ROS中提供了tf包来进行坐标变换的各种操作（http://wiki.ros.org/tf）

使用如下命令启动例子：

roslaunch mobot_urdf mobot_w_arm.launch

可以查看tf的话题信息

rostopic info tf

得到如下结果

Type: tf2_msgs/TFMessage

这是消息类型，进一步，可以查看消息

rosmsg show tf2_msgs/TFMessage

得到消息包含的变量

geometry_msgs/TransformStamped[] transforms
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/Transform transform
geometry_msgs/Vector3 translation
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation
float64 x
float64 y
float64 z
float64 w

可以看出，ROS变换数据与4×44×4的其次坐标变换矩阵并不一致，但是它包含了相同的信息，甚至更多。ROS变换信息包含了一个三维向量（表示位移）和一个四元数（表示旋转）。此外，变换消息还有时间戳，子坐标系和父坐标系等信息。

有许多节点向tf话题发布消息。每一个发布者都发布子坐标系相对于父坐标系的变换关系。在当前的例子中，Gazebo向tf发布消息，因为urdf文件中加入了差分驱动插件：

<gazebo>
<plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
<alwaysOn>true</alwaysOn>
<updateRate>100</updateRate>
<leftJoint>right_wheel_joint</leftJoint>
<rightJoint>left_wheel_joint</rightJoint>
<wheelSeparation>${track}</wheelSeparation>
<wheelDiameter>${tirediam}</wheelDiameter>
<torque>200</torque>
<commandTopic>cmd_vel</commandTopic>
<odometryTopic>odom</odometryTopic>
<odometryFrame>odom</odometryFrame>
<robotBaseFrame>base_link</robotBaseFrame>
<publishWheelTF>true</publishWheelTF>
<publishWheelJointState>true</publishWheelJointState>
</plugin>
</gazebo>

运行

rostopic hz tf

可以查看tf的发布频率

subscribed to [/tf]
WARNING: may be using simulated time
average rate: 302.703
min: 0.000s max: 0.013s std dev: 0.00469s window: 225
average rate: 301.118
min: 0.000s max: 0.013s std dev: 0.00471s window: 459
average rate: 301.000
min: 0.000s max: 0.013s std dev: 0.00469s window: 693
average rate: 300.747
min: 0.000s max: 0.014s std dev: 0.00468s window: 927
average rate: 300.517
min: 0.000s max: 0.014s std dev: 0.00469s window: 1164
average rate: 300.429
min: 0.000s max: 0.015s std dev: 0.00470s window: 1401
average rate: 300.368
min: 0.000s max: 0.015s std dev: 0.00469s window: 1635
average rate: 300.322
min: 0.000s max: 0.015s std dev: 0.00468s window: 1866
average rate: 300.000
min: 0.000s max: 0.015s std dev: 0.00468s window: 2101

大概在300Hz。

查看tf消息的数据可以使用

rostopic echo tf

得到

frame_id: "base_link"
child_frame_id: "left_wheel"
transform:
translation:
x: -4.47909858294e-07
y: 0.282573167674
z: 0.165125664005
rotation:
x: -0.452744260782
y: 0.54316792808
z: 0.543160481729
w: 0.452733837175
---
transforms:
-
header:
seq: 0
stamp:
secs: 1442
nsecs: 127000000
frame_id: "base_link"
child_frame_id: "right_wheel"
transform:
translation:
x: -2.77368998557e-07
y: -0.282574928023
z: 0.165114429016
rotation:
x: -0.455898707514
y: 0.54052002351
z: 0.54051615107
w: 0.455891174628
---

在ROS中，旋转变换通常通过四元数来表示，四元数表示旋转是旋转矩阵的替代。它可以变换成3×33×3的旋转矩阵，或者直接进行四元数操作的坐标变换。四元数比旋转矩阵更加简洁，并且有吸引人的数学特性。

关于四元数的细节可以参考其他图书2,[^foot3]，在这里知道四元数与旋转矩阵之间有联系，并且可以相互转换就足够了。

计算旋转变换需要关节角度值，可以通过如下命令得到：

rostopic echo joint_states

Transform listener

Transform listener用于执行从一个坐标系变换到另一个坐标系的变换。 tf_listener通常作为一个独立的线程启动，这一线程订阅tf话题，并且从父子变换关系中集成一个运动学树。由于 tf_listener包含所有的变换发布消息，所以它能够进行特殊的查询比如右手掌相对于左摄像头的变换。查询所回应的是变换消息。

有关tf listener使用的例子都在example_tf_listener包中。主要包含三个代码文件，example_tf_listener.h定义了一个DemoTfListener类，example_tf_listener_fncs.cpp包括类方法的实现；example_tf_listener.cpp中有主程序入口。主程序如下：

//example_tf_listener.cpp:
//wsn, March 2016
//illustrative node to show use of tf listener, with reference to the simple mobile-robot model
// specifically, frames: odom, base_frame, link1 and link2

// this header incorporates all the necessary #include files and defines the class "DemoTfListener"
#include "example_tf_listener.h"
using namespace std;

//main pgm to illustrate transform operations

int main(int argc, char** argv) {
// ROS set-ups:
ros::init(argc, argv, "demoTfListener"); //node name
ros::NodeHandle nh; // create a node handle; need to pass this to the class constructor
ROS_INFO("main: instantiating an object of type DemoTfListener");
DemoTfListener demoTfListener(&nh); //instantiate an ExampleRosClass object and pass in pointer to nodehandle for constructor to use

tf::StampedTransform stfBaseToLink2, stfBaseToLink1, stfLink1ToLink2;
tf::StampedTransform testStfBaseToLink2;

tf::Transform tfBaseToLink1, tfLink1ToLink2, tfBaseToLink2, altTfBaseToLink2;

demoTfListener.tfListener_->lookupTransform("base_link", "link1", ros::Time(0), stfBaseToLink1);
cout << endl << "base to link1: " << endl;
demoTfListener.printStampedTf(stfBaseToLink1);
tfBaseToLink1 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink1);

demoTfListener.tfListener_->lookupTransform("link1", "link2", ros::Time(0), stfLink1ToLink2);
cout << endl << "link1 to link2: " << endl;
demoTfListener.printStampedTf(stfLink1ToLink2);
tfLink1ToLink2 = demoTfListener.get_tf_from_stamped_tf(stfLink1ToLink2);

demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);
cout << endl << "base to link2: " << endl;
demoTfListener.printStampedTf(stfBaseToLink2);
tfBaseToLink2 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink2);
cout << endl << "extracted tf: " << endl;
demoTfListener.printTf(tfBaseToLink2);

altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;
cout << endl << "result of multiply tfBaseToLink1*tfLink1ToLink2: " << endl;
demoTfListener.printTf(altTfBaseToLink2);

if (demoTfListener.multiply_stamped_tfs(stfBaseToLink1, stfLink1ToLink2, testStfBaseToLink2)) {
cout << endl << "testStfBaseToLink2:" << endl;
demoTfListener.printStampedTf(testStfBaseToLink2);
}
cout << endl << "attempt multiply of stamped transforms in wrong order:" << endl;
demoTfListener.multiply_stamped_tfs(stfLink1ToLink2, stfBaseToLink1, testStfBaseToLink2);

geometry_msgs::PoseStamped stPose, stPose_wrt_base;
stPose = demoTfListener.get_pose_from_transform(stfLink1ToLink2);
cout << endl << "pose link2 w/rt link1, from stfLink1ToLink2" << endl;
demoTfListener.printStampedPose(stPose);

demoTfListener.tfListener_->transformPose("base_link", stPose, stPose_wrt_base);
cout << endl << "pose of link2 transformed to base frame:" << endl;
demoTfListener.printStampedPose(stPose_wrt_base);

return 0;
}

在第17行将DemoTfListener类进行了实例化。类的对象中有一个transform listener，在主程序中有4个位置使用ransform listener：24，29,34和57行。

第一个实例化的24行：

demoTfListener.tfListener_->lookupTransform("base_link", "link1", ros::Time(0), stfBaseToLink1);

transform listener 订阅tf话题，并不断尝试从所有发布的父子空间关系中组装成最可能的变换链。一旦 transform listener 被实例化，并且有一个简短的时间来收集已经发布的变换关系，就有很多方法可以使用。lookupTransform方法可以用来找到两个坐标系之间的变换关系（比如base_link和link1）。lookupTransform方法的结果传给tf::StampedTransform类型的对象stfBaseToLink1。这种带有标记的变换包含的原点位移和旋转。

lookupTransform()函数使用特定参数来定义感兴趣的坐标系与期望的坐标系。参数ros::Time(0) 说明当前坐标系是期望的。

对象stfBaseToLink1有一个时间戳，作为参考坐标系和子坐标系的标签，从tf :: StampedTransform对象中提取tf :: Transform并不像预期的那么简单。 函数get_tf_from_stamped_tf（）在DemoTfListener类中定义以进行协助。

tfBaseToLink1 = demoTfListener.get_tf_from_stamped_tf(stfBaseToLink1);

tfBaseToLink1变换是从stfBaseToLink1中提取出来的。这一变换描述的是link1相对于base_frame的位置和旋转的变换。其他几行的实例化操作与此类似。

*操作由tf::Transform 对象定义，所以tfBaseToLink1 和 tfLink1ToLink2 可以乘在一起，在41行所示

altTfBaseToLink2 = tfBaseToLink1*tfLink1ToLink2;

这种操作的意思是将两个变换串联到一起。等同于4×44×4矩阵相乘。altTfBaseToLink2的结果与下面的结果一样

demoTfListener.tfListener_->lookupTransform("base_link", "link2", ros::Time(0), stfBaseToLink2);

然后再从stfBaseToLink2解析得到变换矩阵。

运行这个例子，先启动

roslaunch mobot_urdf mobot_w_arm_and_jnt_pub.launch

然后运行

rosrun robot_state_publisher robot_state_publisher

接着运行

rosrun example_tf_listener example_tf_listener

可以得到如下结果

[ INFO] [1519276137.994933013, 46.997000000]: tf is good

base to link1:
frame_id: base_link
child_frame_id: link1
vector from reference frame to to child frame: -0.27305,0,0.55
orientation of child frame w/rt reference frame:
1,0,0
0,1,0
0,0,1
quaternion: 0, 0, 0, 1

link1 to link2:
frame_id: link1
child_frame_id: link2
vector from reference frame to to child frame: 0,0,1
orientation of child frame w/rt reference frame:
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

base to link2:
frame_id: base_link
child_frame_id: link2
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame:
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

extracted tf:
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame:
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

result of multiply tfBaseToLink1*tfLink1ToLink2:
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame:
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

testStfBaseToLink2:
frame_id: base_link
child_frame_id: link2
vector from reference frame to to child frame: -0.27305,0,1.55
orientation of child frame w/rt reference frame:
0.999801,0,0.0199663
0,1,0
-0.0199663,0,0.999801
quaternion: 0, 0.00998362, 0, 0.99995

attempt multiply of stamped transforms in wrong order:
can't multiply transforms; mismatched frames
link2 is not base_link

pose link2 w/rt link1, from stfLink1ToLink2
frame id = link1
origin: 0, 0, 1
quaternion: 0, 0.00998362, 0, 0.99995

pose of link2 transformed to base frame:
frame id = base_link
origin: -0.27305, 0, 1.55
quaternion: 0, 0.00998362, 0, 0.99995

以上结果可以与主程序进行对比。

geometry_msgs类型与tf类型的转化是很复杂的，example_tf_listener_fncs.cpp中提供了将tf::StampedTransform解析成geometry_msgs::PoseStamped类型变量的方法。如下所示

geometry_msgs::PoseStamped DemoTfListener::get_pose_from_transform(tf::StampedTransform tf) {
//clumsy conversions--points, vectors and quaternions are different data types in tf vs geometry_msgs
geometry_msgs::PoseStamped stPose;
geometry_msgs::Quaternion quat;  //geometry_msgs object for quaternion
tf::Quaternion tfQuat; // tf library object for quaternion
tfQuat = tf.getRotation(); // member fnc to extract the quaternion from a transform
quat.x = tfQuat.x(); // copy the data from tf-style quaternion to geometry_msgs-style quaternion
quat.y = tfQuat.y();
quat.z = tfQuat.z();
quat.w = tfQuat.w();
stPose.pose.orientation = quat; //set the orientation of our PoseStamped object from result

// now do the same for the origin--equivalently, vector from parent to child frame
tf::Vector3 tfVec;  //tf-library type
geometry_msgs::Point pt; //equivalent geometry_msgs type
tfVec = tf.getOrigin(); // extract the vector from parent to child from transform
pt.x = tfVec.getX(); //copy the components into geometry_msgs type
pt.y = tfVec.getY();
pt.z = tfVec.getZ();
stPose.pose.position= pt; //and use this compatible type to set the position of the PoseStamped
stPose.header.frame_id = tf.frame_id_; //the pose is expressed w/rt this reference frame
stPose.header.stamp = tf.stamp_; // preserve the time stamp of the original transform
return stPose;
}

尽管向量和四元数分别定义在了geometry_msgs和tf中，这些类型并不是兼容的。上述代码表示了他们之间的转化。

当实例化一个transform listener时，查找函数应该针对返回的错误进行测试。这在构造函数中实现：

DemoTfListener::DemoTfListener(ros::NodeHandle* nodehandle):nh_(*nodehandle)
{
ROS_INFO("in class constructor of DemoTfListener");
tfListener_ = new tf::TransformListener;  //create a transform listener and assign its pointer
//here, the tfListener_ is a pointer to this object, so must use -> instead of "." operator
//somewhat more complex than creating a tf_listener in "main()", but illustrates how
// to instantiate a tf_listener within a class

// wait to start receiving valid tf transforms between base_link and link2:
// this example is specific to our mobot, which has a base_link and a link2
// lookupTransform will through errors until a valid chain has been found from target to source frames
bool tferr=true;
ROS_INFO("waiting for tf between link2 and base_link...");
tf::StampedTransform tfLink2WrtBaseLink;
while (tferr) {
tferr=false;
try {
//try to lookup transform, link2-frame w/rt base_link frame; this will test if
// a valid transform chain has been published from base_frame to link2
tfListener_->lookupTransform("base_link", "link2", ros::Time(0), tfLink2WrtBaseLink);
} catch(tf::TransformException &exception) {
ROS_WARN("%s; retrying...", exception.what());
tferr=true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("tf is good");
// from now on, tfListener will keep track of transforms; do NOT need ros::spin(), since
// tf_listener gets spawned as a separate thread
}

Transforming ROS datatypes

使用XformUtils进行数据格式转换

ROS节点通过定义的消息类型相互通信。ROS发布者解构消息对象的数据组件，序列化数据并通过话题发送它。对应的subscriber接收连续消息数据并重构消息组件。该过程便于通信，但消息类型对于执行数学运算可能很麻烦。

xform_utils包含一个便利的转换函数库。比如transformPoseToEigenAffine3d()函数可以将geometry_msgs::Pose转换为Eigen::Affine3d类型的数据，如下所示

Eigen::Affine3d XformUtils::transformPoseToEigenAffine3d(geometry_msgs::PoseStamped stPose) {
Eigen::Affine3d affine;
geometry_msgs::Pose pose = stPose.pose;
Eigen::Vector3d Oe;
//ROS_WARN("xformUtils: input pose:");
//printPose(pose);
Oe(0) = pose.position.x;
Oe(1) = pose.position.y;
Oe(2) = pose.position.z;
affine.translation() = Oe;

Eigen::Quaterniond q;
q.x() = pose.orientation.x;
q.y() = pose.orientation.y;
q.z() = pose.orientation.z;
q.w() = pose.orientation.w;
Eigen::Matrix3d Re(q);

affine.linear() = Re;
affine.translation() = Oe;
//printAffine(affine);
return affine;
}

使用这些函数需要在package.xml中包含xform_utils依赖dependency，源码应该包含头文件 #include < xform_utils/xform_utils.h >。

小结

这篇需要结合代码学习。

坐标变换在机器人学中是普遍存在的，相应的，ROS中也有大量的工具来处理坐标变换的问题。不过，这些可能有点令人困惑，因为有多种表示形式。 tf库提供了一个tf listener，可以在节点内方便地使用，以监听所有转换发布的消息并将其组装成连贯的链。 tf listener可以查询任何两个连接帧之间的空间关系。 并用最新的变换信息进行响应。 相比之下，规划师会考虑假设关系。 因此，tf对于规划目的不太有用，因此可能需要独立计算运动转换以进行规划。 此外，虽然tf是动态更新的，但有一个潜在的限制是组合的转换链可能稍微有时间延迟。 因此，tf结果不应用于时间关键的反馈环路中（例如，对于力控制，可能需要计算自己的运动转换）。



A Systematic Approach to Learning Robot Programming with ROS ↩
FundaJ, PaulRP. A comparison of transforms and quaternions in robotics. Proceedings. 1988 IEEE International Conference on Robotics and Automation, pages 886–891 vol. 2, Apr 1988.

[^foot3]: RichardM. Murray, Zexiang Li, and S. Boca Raton: Shankar Sastry. A mathematical introduction to robotic manipulation. CRC Press; 1994. ↩