tf 的运用

include文件：

[cpp] view plaincopy

1. </pre><pre name="code" class="cpp">// For transform support  
2. #include "tf/transform_broadcaster.h"  
3. #include "tf/transform_listener.h"  
4. #include "tf/message_filter.h"  
5. #include "tf/tf.h"  
6. #include "message_filters/subscriber.h"  

某时刻机器人在地图上的位置：

当机器人在移动过程中，tf会不断接收 base_link->odom 的位置关系信息，这些信息是根据时间不断变化并被记录下来的。当其它节点需要获取某个时间点上的 base_link的位置时就可以通过下面的方法查询：

x, y, yaw 就是base_link 在t 时刻在地图上的位置：

[cpp] view plaincopy

1. bool getOdomPose(tf::Stamped<tf::Pose>& odom_pose,  
2.                       double& x, double& y, double& yaw,  
3.                       const ros::Time& t, const std::string& base_link)  
4. {  
5.   // Get the robot's pose  
6.   tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),  
7.                                            tf::Vector3(0,0,0)), t, base_link );  
8.   try  
9.   {  
10.     tf_ = new tf::TransformListener();  

[cpp] view plaincopy

1.     tf_->transformPose(odom_frame_id_, ident, odom_pose);  
2.   }  
3.   catch(tf::TransformException e)  
4.   {  
5.     ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());  
6.     return false;  
7.   }  
8.   x = odom_pose.getOrigin().x();  
9.   y = odom_pose.getOrigin().y();  
10.   double pitch,roll;  
11.   odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);  
12.   
13.   return true;  
14. }  

机器人某个位置相对map的位置关系：

机器人是矩形，四个角儿相对中心的位置已知，获取四个角相对map的位置

[cpp] view plaincopy

1. tf::Stamped<tf::Pose> corner1(  
2.     tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, -0.45, 0.0)),  
3.     ros::Time(0), "base_link");  
4. tf::Stamped<tf::Pose> corner2(  
5.     tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.30, 0.45, 0.0)),  
6.     ros::Time(0), "base_link");  
7. tf::Stamped<tf::Pose> corner3(  
8.     tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, -0.45, 0.0)),   
9.     ros::Time(0), "base_link");  
10. tf::Stamped<tf::Pose> corner4(  
11.     tf::Pose(tf::Quaternion(0, 0, 0, 1), tf::Vector3(-0.30, 0.45, 0.0)),   
12.     ros::Time(0), "base_link");  
13. transform_listener = new tf::TransformListener();  
14. tf::Stamped<tf::Pose> transformed_corner_1;  
15. transform_listener.transformPose("map", corner_1, transformed_corner_1);  
16. tf::Stamped<tf::Pose> transformed_corner_2;  
17. transform_listener.transformPose("map", corner_2, transformed_corner_2);  
18. tf::Stamped<tf::Pose> transformed_corner_3;  
19. transform_listener.transformPose("map", corner_3, transformed_corner_3);  
20. tf::Stamped<tf::Pose> transformed_corner_1;  
21. transform_listener.transformPose("map", corner_4, transformed_corner_4);  

随机器人移动点在t+1时刻的位置

已知 t 时刻的位置是 pose_old，求t+1 时刻的位置 pose_new

[cpp] view plaincopy

1.   tf_ = new tf::TransformListener();  
2.   tf::StampedTransform tx_odom;  
3.   try  
4.   {  
5.     tf_->lookupTransform(base_frame_id_, ros::Time::now(),  
6.                          base_frame_id_, msg.header.stamp,  
7.                          global_frame_id_, tx_odom);  
8.   }  
9.   catch(tf::TransformException e)  
10.   {  
11.     ROS_WARN("Failed to transform initial pose in time (%s)", e.what());  
12.     tx_odom.setIdentity();  
13.   }  
14.     
15.   tf::Pose pose_old, pose_new;  
16.   tf::poseMsgToTF(msg.pose.pose, pose_old);  
17.   pose_new = tx_odom.inverse() * pose_old;  
18.   
19.   
20.   // Transform into the global frame  
21.   ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",  
22.            ros::Time::now().toSec(),  
23.            pose_new.getOrigin().x(),  
24.            pose_new.getOrigin().y(),  
25.            getYaw(pose_new));  

这里认为global_frame_id是不动的，pose_old和pose_new都是在global_frame_id坐标系下的坐标。但是pose_old描述的物体是随着base_frame_id同步移动的

关于fixed frame的解释：2.3 Transforms in Time

相对角度的转换Quaternion

当base_link代表机器人时，激光扫描仪laser_scan安装的角度与base_link不平行，即激光数据的零度不对应机器人的正前方零度。已知 laser_scan->angle_min  和 laser_scan->angle_increment 为激光数据信息，转换角度到base_link的位置代码如下，该算法可以考虑到激光器上下颠倒安装的情况导致angle_increment为负：

[cpp] view plaincopy

1. tf::Quaternion q;  
2. q.setRPY(0.0, 0.0, laser_scan->angle_min);  
3. tf::Stamped<tf::Quaternion> min_q(q, laser_scan->header.stamp,  
4.                                   laser_scan->header.frame_id);  
5. q.setRPY(0.0, 0.0, laser_scan->angle_min + laser_scan->angle_increment);  
6. tf::Stamped<tf::Quaternion> inc_q(q, laser_scan->header.stamp,  
7.                                   laser_scan->header.frame_id);  
8. try  
9. {  
10.   tf_->transformQuaternion(base_frame_id_, min_q, min_q);  
11.   tf_->transformQuaternion(base_frame_id_, inc_q, inc_q);  
12. }  
13. catch(tf::TransformException& e)  
14. {  
15.   ROS_WARN("Unable to transform min/max laser angles into base frame: %s",  
16.            e.what());  
17.   return;  
18. }  
19.   
20. double angle_min = tf::getYaw(min_q);  
21. double angle_increment = tf::getYaw(inc_q) - angle_min; //考虑到了激光器上下颠倒安装的情况导致为负数  

已知 W->B 和B->A的坐标转换，求W->A的坐标转换

ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法

有时间差的lookupTransform

ros上的详细教程

turtle1和turtle2都是 world 的child frame. turtle1->world 和turtle2->world 的tf都不断发布的，现在需要知道这样的一个transform转换关系：

5秒中之前turtle1相对与现在的turtle2的位置关系

[cpp] view plaincopy

1. try{  
2.     ros::Time now = ros::Time::now();  
3.     ros::Time past = now - ros::Duration(5.0);  
4.     listener.waitForTransform("/turtle2", now,  
5.                               "/turtle1", past,  
6.                               "/world", ros::Duration(1.0));  
7.     listener.lookupTransform("/turtle2", now,  
8.                              "/turtle1", past,  
9.                              "/world", transform);  

得到的转换结果可以这样理解, ( transform.getOrigin().x(), transform.getOrigin().y() ) 是以turtle2为原点的XY平面上turtle1的坐标。