ROS学习笔记：tf的学习和使用（二）

1.增加一个坐标系

前面我们的效果是让turtle2跟着turtle1走，那么做到有一定布局地跟随呢？事实上原理比较简单，我们只需再构造一个turtle1的子坐标系，然后让turtle2跟着这个子坐标系走即可。

需要注意的是，每个子坐标系都只能有一个父坐标系，而一个坐标系是可以有多个子坐标系的。

 $ roscd learning_tf

src/frame_tf_broadcaster.cpp

   1 #include /ros.h>
   2 #include /transform_broadcaster.h>
   3 
   4 int main(int argc, char** argv){
   5   ros::init(argc, argv, "my_tf_broadcaster");
   6   ros::NodeHandle node;
   7 
   8   tf::TransformBroadcaster br;
   9   tf::Transform transform;
  10 
  11   ros::Rate rate(10.0);
  12   while (node.ok()){
  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
  16     rate.sleep();
  17   }
  18   return 0;
  19 };

核心代码解释

  13     transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
  14     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
  15     br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));

setOrigin是设置这个子坐标系与父坐标系的初始位置关系，三个参数分别问x，y，z，z一般设置为0.

setRotation是设置这个子坐标系与父坐标系的角度关系，前三个参数为pitch,row,yaw，最后一个参数为角速度。

最后一行就是继承的父坐标系和子坐标系名称和时间起点。

修改src/turtle_tf_listener.cpp

   1   listener.lookupTransform("/turtle2", "/carrot1",
   2                            ros::Time(0), transform);

修改CMakeLists.txt

add_executable(frame_tf_broadcaster src/frame_tf_broadcaster.cpp)
target_link_libraries(frame_tf_broadcaster ${catkin_LIBRARIES})

在launch文件中添加

              name="broadcaster_frame" />

编译运行

 $ catkin_make

 $ roslaunch learning_tf start_demo.launch

事实上，我们对参数进行一些简单的修改就能看到一些有趣的效果

   1     transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );
   2     transform.setRotation( tf::Quaternion(0, 0, 0, 1) );

例如改变初始位置为一个时间的正余弦函数，那么很自然的，turtle2便会绕turtle1做一个圆周运动。

2.tf和时间漫游

在之前的例子里，我们的lookupTransform()函数的时间参数都是ros::Time(0)，这里的0就意味着是最近时刻采样的坐标系。这个参数是可以变化的，比如我们将其改为ros::Time::now()，now的意思就是当前的坐标系。但是这时候如果直接运行就睡出现如下错误

这是因为一个坐标系从一个节点广播到被另一个节点接收，是需要一定时间的，所以直接订阅当前时间节点的坐标系是找不到的，我们可以做如下修改，等待一个时间确保广播的数据已经被收到。

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

之后再运行就会如我们预期一样。

我们可以再做一些有趣的尝试，比如turtle2不再到达turtle1目前的位置，而是到达turtle2 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);

但是结果貌似和我们的预期有很大差距

开始的诡异轨迹可以理解为我们并没有turtle1在0时刻以前的数据，那之后呢？所以，显然这里面有一个隐藏的错误。

我们回过头来看上面那段代码，你会发现，我们输入量是5秒之前的turtle1，但是，我们忽略了我们控制的也是5秒之前的turtle2。

很显然，我们希望达到的目的是利用5秒之前的turtle1的信息来控制当前的turtle2的位置，因此我们需要对代码做一些修改

  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);

这样结果就和我们预期的一样了。