tf 使用

1. 发布自己的tf：

   

其实就是发布你建立的坐标系。步骤如下：
1.定义一个广播，相当于发布话题时定义一个发布器，还是以官方的小乌龟例程为例：

static tf::TransformBroadcaster br；

2.定义存放转换信息（平动，转动）的变量

tf::Transform transform;

3.设置坐标原点

transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );

需要注意：
（1）setOrigin()函数的参数类型需要为tf::Vector3类型
（2）假设是要发布一个子坐标系为”turtle1”父坐标系为“world”，那么其中(msg->x,msg->y,0.0)是指“turtle1”的坐标原点在“world”坐标系下的坐标。
4.定义旋转

 // 根据 欧拉角设置
 tf::Quaternion q;
  q.setRPY(0, 0, msg->theta);
  transform.setRotation(q);
 // 根据四元数设置
transform.setRotation( tf::Quaternion(0, 0, 0, 1) ); 

需要注意:
（1）setRPY()函数的参数为”turtle1”在“world”坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)
（2）为了确保转换正确强烈建议，在转换完后，运行下程序，打开rviz下使用确认下转换是否正确，我在实验中有一次，明明依次填入了三个角度，但是在rviz下发现并不正确，pitch,yaw还存在一个负号的关系（楼主对旋转角的正负号向来头晕，所以都是直接打开rviz，直接试，直观而且准确）。
5.将变换广播出去

br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));

其中，
（1）transform:存储变换关系的变量；
（2）ros::Time::now():广播tf使的时间戳；
（3）“world”:父坐标系的名字；
（4）turtle_name:子坐标系的名字，这里因为子坐标系有两个，所以它定义了字符串用于存放名字，方便切换，通常我们把名字填到引号里就可以了。
总结一下，假设你在机器人上应用，如果你知道机器人的位置x,y,z,与三个旋转角roll,pitch,yaw就可以广播一个tf了，如果在是平面移动机器人，则只需要知道x,y与yaw即可。

二、 获取 tf  关系：

 通过监听tf，我们可以避免繁琐的旋转矩阵的计算，而直接获取我们需要的相关信息。
在监听中我最常用两个函数：
（1）lookupTransform（）；
（2）transformPoint（）；

1。  lookupTransform（）：

     

1.函数功能：可以过得两个坐标系之间转换的关系，包括旋转与平移。
2.使用例程：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C++%29
3.主要步骤：
（1）定义监听器；

    tf::TransformListener listener

（2）定义存放变换关系的变量

    tf::StampedTransform transform;

（3）监听两个坐标系之间的变换

   try{
      listener.lookupTransform("/turtle2", "/turtle1",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
    }

注意：1.由于tf的会把监听的内容存放到一个缓存中，然后再读取相关的内容，而这个过程可能会有几毫秒的延迟，也就是，tf的监听器并不能监听到“现在”的变换，所以如果不使用try,catch函数会导致报错：“world” passed to lookupTransform argument target_frame does not exist. ”

2..lookupTransform()函数参数说明
a.官网的例程说的是实现从“/turtle2”到“/turtle1”的转换，但实际中使用时，我发现转换得出的坐标是在“/turtle2”坐标系下的
b.不可以把ros::Time(0)改成ros::time::now()，因为监听做不到实时，会有几毫秒的延迟。ros::Time(0)指最近时刻存储的数据，ros::time::now()则指当下，如果非要使用ros::time::now,则需要结合waitForTransform()使用，具体见:
http://wiki.ros.org/tf/Tutorials/tf%20and%20Time%20%28C++%29
实际中ros::Time(0)大多数情况下可以满足要求。
c.最后转换关系存入transform中

（4）使用变换
通常我们使用的是点的信息

transform.getOrigin().x()
transform.getOrigin().y()

可以得到在turtle1坐标系的原点，在turtle2坐标系下的位置。

transform.getRotation().getW();
transform.getRotation().getX();
transform.getRotation().getY();
transform.getRotation().getZ();

可以得到旋转的四元数，如何直接得到偏航角因为没有用到这个功能所以也就没有去找相关的函数。

（二）transformPoint（）函数使用
在实际应用中我们肯定会需要把在一个坐标系下的点转换到另一个坐标系下，这就需要transformPoint（）函数。
1.函数功能：将一个坐标系下的点的坐标转换到另一个坐标系下。
2.函数参数：如

   listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world)；

（1）其中m_normal_pose数据类型为 geometry_msgs::PointStamped，
其中需要定义m_normal_pose.header.frame_id即该点所属的坐标系
（2）而“PTAM_world”则指，我要将m_normal_pose转换到“PTAM_world”坐标系下。
pose_PTAM_world是转换的结果，数据类型同样为geometry_msgs::PointStamped。
3.使用例程：

geometry_msgs::PointStamped turtle1;
turtle1.header.stamp=ros::Time();
turtle1.header.frame_id="turtle1";
turtle1.point.x=1;
turtle1.point.y=2;
turtle1.point.z=3;
geometry_msgs::PointStamped turtle1_world;
        try{
listener_.transformPoint("PTAM_world",turtle1,turtle1_world);
         }
             catch (tf::TransformException &ex) {
          ROS_ERROR("%s",ex.what());
          ros::Duration(1.0).sleep();
    }

说明：同样因为延时，turtle1.header.stamp不能为ros::Time::now();否则会出类似错误

[ERROR] [1456669076.279804500]: Lookup would require extrapolation into the future. Requested time 1456669076.279616253 but the latest data is at time 1456669076.159341977, when looking up transform from frame …

总结，只有开始使用tf了才能逐渐感受到它的方便，另外再建议tf一定要与rviz结合使用，真的直观又方便。

四元数 转 欧拉角：

     tf::Quaternion quat;

     tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,quat);
     double roll,pitch,yaw;
     tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);

好像可以直接  quat.getYaw();

  tf::createQuaternionMsgFromYaw(yaw);

   tf::getYaw(msg->pose.pose.orientation);

  //四元数 -》 欧拉角

  tf::Quaternion  quat;
  tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,quat);
  double roll,pitch,yaw;
  tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);