tf树的一些理解

(yaw is rotation about Z, pitch is rotation about Y, and roll is rotation about X）

---------------------------------------------------------

tf下的发布静态坐标关系：

 

try {
            listener.waitForTransform("target_frame","source_frame",time_stamp, ros::Duration(0.5));
            listener.transformPose("target_frame",pose_in_source_frame, pose_in_target_frame);
       }
catch(tf::TransformException& ex){
             ROS_ERROR("Failed to transform from source to target: %s\n", ex.what());
             return false;
        }

    const geometry_msgs::Pose& pose = xx;
    tf::Pose tf_pose;
    tf::poseMsgToTF(pose, tf_pose);
    double theta = tf::getYaw(tf_pose.getRotation());///绕z

geometry_msgs::PoseStamped robot_pose=xx;

double yaw,pitch,roll;

ros::Rate r(10);
transform.setRotation( tf::Quaternion(robot_pose.pose.orientation.x,
                                               robot_pose.pose.orientation.y,
                                               robot_pose.pose.orientation.z,
                                               robot_pose.pose.orientation.w) );
        transform.getBasis().getEulerYPR(yaw, pitch, roll);

-----------------------------------------------

1.建立和维护一棵tf树。主要是通过不停地向tf树发送各个坐标系之间的转换关系，也就是位置关系，来维护一个tf树。主要用到的指令就是：

 static tf::TransformBroadcaster br;
  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);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

其中：sendTransform(tf::StampedTransform(转换关系, 时间戳, 父坐标系名称（字符串）,子坐标系名称（字符串)））;

注意：一个tf树中必须有一个固定不动的坐标系才行，一般就用世界，其他坐标系都在此基础上进行建立。这样不容易出错。
2.使用一棵tf树。使用tf树主要就是通过订阅的方式，获取某个坐标系的与目标坐标系的转换关系，也可以理解位某个坐标系在另一个坐标系下的原点位置和坐标系轴的角度差。
这个订阅的操作只在我们需要这个位置关系的时候运行即可，不用像1中维护tf树一样，需要一直不停的，即什么时候需要位置关系， 什么时候订阅一下就可以了。
（其实tf树的就是一个ros的订阅与发布的过程，只不过要维护一个tf需要连续不停地发布，而订阅确实我们什么时候想要什么发起即可。）
订阅tf树用到的核心代码：

try {
    listener.waitForTransform(destination_frame, original_frame, ros::Time(0), ros::Duration(10.0) );
    listener.lookupTransform(destination_frame, original_frame, ros::Time(0), transform);
} catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
}

其中：

lookupTransform(destination_frame, original_frame, ros::Time(0), transform);

可以这样理解：original_frame的原点在destination_frame坐标系下的坐标。这样求的就是当前时刻的两坐标系关系。

lookupTransform（）还有另外一种用法，求取制定时刻两坐标系之间的位置关系：

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

}
求取在当前turtle2坐标系下的5秒以前的turtle1坐标系原点的位置。这时候需要指明那个固定的坐标系，即world。官方解释：

The advanced API for lookupTransform() takes six arguments:

1. Give the transform from this frame,
2. at this time ...
3. ... to this frame,
4. at this time.
5. Specify the frame that does not change over time, in this case the "/world" frame, and
6. the variable to store the result in.

3.一些例子：小龟2追着小龟1跑——》小龟2应该往哪个方向走？往小龟1所在的位置走——》小龟1在小龟2坐标系下的坐标是多少？——》通过一个固定的坐标系
world作为中介获得——》用2个broadcaster建立小龟1，小龟2，World之间的相对关系——》小龟1 动了，它在world下的坐标变化——》小龟2通过一个listener获
取小龟1在小龟2 坐标系下的位置——》小龟2朝着小龟1跑过去.
如果去掉world坐标系，直接吧小龟2 当做小龟1的父坐标，这样是不对的，因为小龟2本身也是需要运动的，坐标的关系就乱套了。
tf的建立和维护：
#include
#include
#include

std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){

        static tf::TransformBroadcaster br;
  tf::Transform transform;
  transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );///xyz位置关系，把小龟的位置进行更新
  tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);///角度关系，把小龟的角度进行更新
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));///小龟的坐标是在世界坐标系下的，所以小龟坐标系是世界坐标系的子坐标系
  ///tf::StampedTransform(位置关系，时间戳，父坐标系的名字，子坐标系的名字）
                          ///sendTransform(StampedTransform())就将父子坐标系的位置关系发布出去了
                          ///下一步我们通过定于tf这个“话题”来得知world和小龟的位置关系，即小龟的世界坐标
                          ///我们需要不停的向tf树发布这个位置关系，才能保持住这棵树
}

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);///这里从话题订阅来消息，内容就是小龟的位置，进入回调函数，msg这个变量里就是小龟的位置,我们这里订阅的这个位置信息就是在世界坐标系下的！！！
     ///所以直接改成turtle2为turtle1的父坐标系是不对的
  ros::spin();
  return 0;
};

tf树的使用（订阅）：
#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);

  tf::TransformListener listener;

  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
   try {
    listener.waitForTransform("/turtle2", "/carrot1",ros::Time(0), ros::Duration(10.0) );///不要用ros::Time::now(）因为now是当前时刻，而Time（0）是最近的tf树变动的时刻
    listener.lookupTransform("/turtle2", "/carrot1",ros::Time(0), transform);///对于坐标系而言，RT中的T就是一个的原点在另一个下的坐标的位置了，这里求的是turtle2在turtle1下，最近一个时刻的，位置关系
    /*3  time travel with tf

Now, instead of making the second turtle Go to where the first turtle is now, make the second turtle go to where the first turtle was 5 seconds ago

这一节我们不让第二只乌龟跟随这第一只乌龟运动，而是让第二只乌龟跟着第一只乌龟五秒前的轨迹运动。

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

The advanced API for lookupTransform() takes six arguments:

    Give the transform from this frame,
    at this time ...
    ... to this frame,
    at this time.
    Specify the frame that does not change over time, in this case the "/world" frame, and
    the variable to store the result in.

lookupTransform()中的六个部分，寻找turtle2的现在的轨迹，寻找turtle1五秒以前的轨迹，world框架不随时间变换而转变，而框架turtle2跟随的是turtle1五秒前的轨迹，即可以实现功能。


*/
} catch (tf::TransformException ex) {
    ROS_ERROR("%s",ex.what());
}

    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;
};
/*
这是因为turtle2需要非零时间来生成并开始发布tf帧。 因此，第一次请求现在/ turtle2帧可能不存在，
当请求转换时，转换可能不存在，并且第一次失败。 第一次变换后，所有的变换都存在，并且乌龟的行为如预期的那样。*/

3.添加一个额外的坐标
#include
#include

int main(int argc, char** argv){
  ros::init(argc, argv, "frame_tf_broadcaster");
  ros::NodeHandle node;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    //transform.setOrigin( tf::Vector3(0.0, 2.0, 0.0) );
   // transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()), 2.0*cos(ros::Time::now().toSec()), 0.0) );///绕着turtle1转，turtle1走也跟着走，这是因为turtle1是carrot1的坐标系，turtle的“大“坐标动了carrot1的大坐标也要动。
    ///地球和月亮的关系！！！
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};