文章目录

1. tf数据类型¹

在tf/transform_datatypes.h中定义，基本数据类型有：Quaternion, Vector, Point, Pose, Transform

2. tf坐标变换²

Frame是坐标系统，在ROS里都是以3D形式存在，右手原则，X向前，Y向左，Z向上

Points在坐标系以tf::Point形式表达，等同与bullet类型的btVector3

在坐标系W里，坐标的点p，可以用：Wp

3. tf广播变换³

void sendTransform(const StampedTransform & transform);
 void sendTransform(const geometry_msgs::TransformStamped & transform);
 

4. 使用已发布的变换⁴

核心方法：

（4）waitForTransform() 函数
返回bool值，评估变换是否有效

（5）lookupTransform() 函数

transformDATA 方法
tf::TransformListener类的主要目的是在坐标系间进行变换数据。

5. 异常⁵

6. 编写tf广播⁶

创建的功能包需要依赖于tf、roscpp、rospy，以小乌龟Demo为例，还需要添加依赖turtlesim

// tf包提供了TransformBroadcaster的实现，以帮助使发布变换的任务更容易
 // 要使用TransformBroadcaster，我们需要包含tf/transform_broadcaster.h头文件#include
 #include
 #includeusing namespace std;
 std::string turtle_name;void poseCallback(const turtlesim::PoseConstPtr& msg)
 {// TF广播器// 创建一个TransformBroadcaster对象，我们稍后将使用它通过线路发送转换static tf::TransformBroadcaster br;// 创建一个Transform对象，并将信息从2D乌龟姿势复制到3D变换中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);// 发布坐标变换// 使用TransformBroadcaster发送转换需要四个参数；首先传递转换对象，采用ros::Time::now()来给发布的变换一个时间戳；然后需要传递创建的链接的父框架名称，此处为“world”；最后需要传递正在创建的链接的子框架名称，此处为乌龟本身名称br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtle_name));
 }int main(int argc,char** argv)
 {// 初始化节点ros::init(argc, argv, "my_tf_broadcaster");if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;}turtle_name = argv[1];ros::NodeHandle node;ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);ros::spin();return 0;}

注意：sendTransform和StampedTransform具有父对象和子对象的相反顺序。

使用tf_echo工具检查位姿是否广播到tf:
rosrun tf tf_echo /world /turtle1

7. 编写tf监听器⁷

// tf包提供了TransformListener的实现，以帮助使接收变换的任务更容易
 // 要使用TransformListener，我们需要包括tf/transform_listener.h头文件#include 
 #include 
 #include 
 #include int main(int argc, char** argv)
 {ros::init(argc, argv, "my_tf_listener");ros::NodeHandle node;ros::service::waitForService("spawn");ros::ServiceClient add_turtle = node.serviceClient("spawn");turtlesim::Spawn srv;add_turtle.call(srv);ros::Publisher turtle_vel = node.advertise("turtle2/cmd_vel", 10);// 创建一个TransformListener对象,一旦监听器被创建，它开始接收tf转换，并缓冲它们长达10秒;TransformListener对象应该被限定为持久化，否则它的缓存将无法填充，并且几乎每个查询都将失败;一个常见的方法是使TransformListener对象成为一个类的成员变量tf::TransformListener listener;ros::Rate rate(10.0);while (node.ok()){tf::StampedTransform transform;try{//  查询监听器进行特定的转换,从/turtle2坐标系开始变换到/turtle1坐标系,变换的时间，提供ros::Time(0)即会给出最近的可用的变换,结果存放的变换对象listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);}catch (tf::TransformException &ex) {ROS_ERROR("%s",ex.what());ros::Duration(1.0).sleep();continue;}// 变换用于计算龟的新的线性和角速度，基于它与龟的距离和角度。// 新的速度发布在话题"turtle2/cmd_vel"中，turtlesim将使用它来更新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;
 };

8. 增加坐标系⁸

以乌龟为例子，添加一个新坐标系carrot1到turtle1

#include 
 #include int main(int argc, char** argv){ros::init(argc, argv, "my_tf_broadcaster");ros::NodeHandle node;tf::TransformBroadcaster br;tf::Transform transform;ros::Rate rate(10.0);while (node.ok()){创建transform，从父系turtle1到子系carrot1，carrot1离turtle1 两米远transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );transform.setRotation( tf::Quaternion(0, 0, 0, 1) );br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));rate.sleep();}return 0;
 };
 

9. 深入Time和TF⁹

值得注意的是，如果只使用lookupTransform()在运行时程序会报错，这是因为 每个监听器有一个缓冲区，它存储来自不同tf广播者的所有坐标变换。 当广播者发出变换时，变换进入缓冲区之前需要一些时间（通常是几个毫秒），所以在时间“now”请求坐标系变换时等待几毫秒再获得该信息便解决了此问题。

 try{ros::Time now = ros::Time::now();listener.waitForTransform("/turtle2", "/turtle1", now, ros::Duration(3.0));listener.lookupTransform("/turtle2", "/turtle1",now, transform);
 

1. 需要等待变换从坐标系turtle2
2. 到坐标系turtle1
3. 在now时间
4. 超时时间，不要等待超过此最大持续时间

注意：使用ros::Time::now()是为了这个例子。通常这将是希望被转换的数据的时间戳。waitForTransform()实际上会阻塞直到两个海龟之间的变换可用（这通常需要几毫秒）或者如果变换不可用,直到达到超时。

对于真实的tf用例，使用Time(0)通常是完全正常的。

10. 时间穿梭(Time travel)¹⁰

  try{ros::Time now = ros::Time::now();listener.waitForTransform("/turtle2", "/turtle1",now, ros::Duration(1.0));listener.lookupTransform("/turtle2", "/turtle1",now, transform);
 

同样以小乌龟Demo为例，现在，不是让turtle2去到turtle1当前时间的地方，而让turtle2去turtle1是5秒前的地方：

try{ros::Time past = ros::Time::now() - ros::Duration(5.0);listener.waitForTransform("/turtle2", "/turtle1",past, ros::Duration(1.0));listener.lookupTransform("/turtle2", "/turtle1",past, transform);
 

相对于/turtle2目前的位置，/turtle1 5秒前的姿势是什么?

回答该问题需要依赖于高级API,示例代码如下：

  try{ros::Time now = ros::Time::now();ros::Time past = now - ros::Duration(5.0);listener.waitForTransform("/turtle2", now,"/turtle1", past,"/world", ros::Duration(1.0));listener.lookupTransform("/turtle2", now,"/turtle1", past,"/world", transform);
 

1. 变换从坐标系turtle2
2. 在now时间
3. 到turtle1坐标系
4. 在past时间
5. 指定不随时间改变的坐标系，这里是world
6. 变换结果保存的变量

---

1. https://www.ncnynl.com/archives/201702/1305.html ↩︎

2. https://www.ncnynl.com/archives/201702/1306.html ↩︎

3. https://www.ncnynl.com/archives/201702/1307.html ↩︎

4. https://www.ncnynl.com/archives/201702/1308.html ↩︎

5. https://www.ncnynl.com/archives/201702/1309.html ↩︎

6. https://www.ncnynl.com/archives/201702/1310.html ↩︎

7. https://www.ncnynl.com/archives/201702/1311.html ↩︎

8. https://www.ncnynl.com/archives/201702/1312.html ↩︎

9. https://www.ncnynl.com/archives/201702/1313.html ↩︎

10. https://www.ncnynl.com/archives/201702/1314.html ↩︎