ROS-TF的使用（常用功能）

tf 使用_人非人1991的博客-CSDN博客

一、TF中的数据格式：

这些数据格式全都是class

头文件：#include <tf/transform_datatypes.h>基本上可以包含所有的tf数据类型

（1）tf::Vector3

tf::Vector3 point;

默认构造一个对象：tf::Vector3（float x, float y, float z）

对于tf::Vector3对象还有set与get方法。

1)举例：构造一个xyz分别为0,0,0的对象  tf::Vector3(0,0,0)

2)举例：使用geometry_msgs::PoseStampedPtr pose构造一个对象

tf::Vector3(pose->pose.position.x,pose->pose.position.y,pose->pose.position.z)

（2）tf::Quaternion

tf::Quaternion q;

1) 举例：使用geometry_msgs::PoseStampedPtr pose构造一个对象

tf::quaternionMsgToTF(pose->pose.orientation,q);   //把geomsg形式的四元数转化为tf形式，得到q

2) q.setRPY(Rtheta,Ptheta,Ytheta);//根据已经知道的欧拉角进行设置q，分别是child_frame绕着frame坐标系下的roll(绕X轴)，pitch(绕Y轴)，yaw(绕Z轴)

已知q，获取roll,pitch,yaw

tf::Matrix3x3(q).getRPY(roll,pitch,yaw);

3)tf_q = tf::createQuaternionFromYaw( theta ); theta是绕z轴旋转角,其他两个轴旋转角为0;

tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);

如要生成ros_q,可以直接使用 :ros_q = tf::createQuaternionMsgFromYaw(theta);

ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);

4) q = tf::Quaternion(0,0,0,1);//直接通过参数设置q，0,0,0,1代表没有任何旋转

拓展：把tf四元数转化为geomsg四元数

geometry_msg::Quaternion msg_q;

tf::quaternionTFToMsg(tf_q,msg_q);

(3)tf::Transform

tf::Transform transform

1) transform = tf::Transform( tf::Quaternion(0,0,0,1) , tf::Vector3(0,0,0) );   //使用参数初始化，得到transform

2) transform. setRotation( tf_q );

transform.setOrigin( tf_point );    //通过set方法得到transform

(4)tf::StampedTransform

这种类型是由tf::Transform作为父类继承而来的

tf::StampedTransform stamped_transform

stamped_transform = tf::StampedTransform( transform, ros::Time::now(), "frame", "child_frame");     //使用参数初始化得到

可以直接获取stamped_transform.getRotation(),   stamped_transform.getOrigin();

二、tf::TransformBroadcaster

发布TF

头文件：#include< tf / transform_broadcaster.h  >

tf::TransformBroadcaster broadcaster;    //定义发布器

broadcaster.sendTransform( stamped_transform ) ;   //发布一个tf，stamped_transform是一个tf::StampedTransform类型的数据。

三、tf::TransformListener

头文件：#include< tf / transform_listener.h>

tf::TransformListener  listener   //定义监听器     
（1）lookuoTransform();   //获取tf

tf::StampedTransform stamped_transform;   //定义存放变换关系的变量
try
{
//得到child_frame坐标系原点，在frame坐标系下的坐标与旋转角度
      listener.lookupTransform("frame", "child_frame",ros::Time(0), stamped_transform);                   
}
    catch (tf::TransformException &ex)
{
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
      continue;
}

获取到stamped_transform后可以使用这些信息

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

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

（2）transformPoint()

在实际应用中需要将一个坐标系下的点转化到另一个坐标系下

//将child_frame坐标系下的Point1，转化到frame坐标系下，存储在point2中
geometry_msgs::PointStamped point1;
point1.header.stamp=ros::Time();
point1.header.frame_id="child_frame";
point1.point.x=1;
point1.point.y=2;
point1.point.z=3;
geometry_msgs::PointStamped point2;
try
{
     listener.transformPoint("frame",point1,point2);
}
catch (tf::TransformException &ex) 
{
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
 }

四、四元数转欧拉角

/*********四元数转欧拉角********/
tf::Quaternion tf_q;
tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_q);
double roll,pitch,yaw;
tf::Matrix3x3(tf_q).getRPY(roll,pitch,yaw);

//四元数转yaw角
tf::getYaw(tf_q);
tf::getYaw(ros_q);

/********欧拉角转四元数*****/
ros_q = tf::createQuaternionMsgFromYaw(yaw);
rt_q = f::createQuaternionFromYaw(yaw);
ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);
tf_q.setRPY(roll,pitch,yaw);

void GetXYZWFromRPY(double roll, double pitch, double yaw, double& x, double& y, double& z, double& w)
{
  roll /= 2;
  pitch /= 2;
  yaw /= 2;
  x = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);
  y = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);
  z = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);
  w = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
}

五、已知tf::Transform tf 和其中一个坐标系下的 pose 可以将其转化到另一个坐标系下

geometry_msgs::Pose transformPose(geometry_msgs::Pose &pose, tf::Transform &tf)
{
    tf::Pose tf_pose;
    tf::poseMsgToTF(pose,tf_pose);

    tf_pose = tf * tf_pose;

    geometry_msgs::Pose ros_pose;
    tf::poseTFToMsg(tf_pose, ros_pose);

    return ros_pose;
}