ROS-坐标变换（tf2）
reference
tf2常用数据类型与常用函数汇总 lixiang csdnblog
tf2_ros::Buffer api，【涉及lookuptransform() transform()内容】
tf2::Quaternion 四元数对象
#include <tf2/LinearMath/Quaternion.h>
tf2::Quaternion q;
q.setRPY(0,0,0);//通过欧拉角的rpy设置四算数
q.normalize(); // 归一化
tf2::Matrix3x3 矩阵
tf2::Quaternion q; q.setRPY(0,0,M_PI/6); // yaw转动30度的四元数
tf2::Matrix3x3 m_basis(q); // 创建旋转矩阵
Matrix3x3 m_basis[0] : 表示矩阵的第一行
Matrix3x3 m_basis[1] : 表示矩阵的第二行
Matrix3x3 m_basis[2] : 表示矩阵的第三行
获取其中的每一个元素
使用m_basis[0] [0],m_basis[0] [1],m_basis[0] [2] —— 获取矩阵第一行的三个元素值；
或者使用m_basis[0].x() ,m_basis[0].y(),m_basis[0].z() —— 获取矩阵第一行的三个元素值；
—— 这其实使用了tf2::Vector的性质；
tf2::Transform 对象
tf2::Transform对象可以进行加减乘除，实现了不同变换之间的运算和连续的多次变换得到的最终变换的结果；
tf2::Transform() 创建tf对象，
tf2::convert()实现相似类型的转换
void AmclNode::savePoseToServer()
{
// We need to apply the last transform to the latest odom pose to get
// the latest map pose to store. We'll take the covariance from
// last_published_pose.
tf2::Transform odom_pose_tf2;
tf2::convert(latest_odom_pose_.pose, odom_pose_tf2); //把一个geometry_msgs/Pose对象，转换为tf2::Transform对象
tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;
double yaw = tf2::getYaw(map_pose.getRotation());
Transform的数据成员
class Transform {
Matrix3x3 m_basis; // 旋转矩阵
Vector3 m_origin; // 平移向量
}
Transform的构造函数
// 默认构造函数，什么操作都没有；
Transform() {}
// 参数1:四元数， 参数2:0标量
explicit TF2SIMD_FORCE_INLINE Transform(const Quaternion& q,
const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
: m_basis(q),
m_origin(c)
{}
// 参数1:3维矩阵， 参数2:0标量
explicit TF2SIMD_FORCE_INLINE Transform(const Matrix3x3& b,
const Vector3& c = Vector3(tf2Scalar(0), tf2Scalar(0), tf2Scalar(0)))
: m_basis(b),
m_origin(c)
{}
Transform 的方法
tf2::Transform 是tf2的tf对象；可以操作的方法主要有如下：
-
Matrix3x3& getBasis() { return m_basis; } //获取该Transform对象的旋转，返回旋转矩阵；
-
Vector3& getOrigin() { return m_origin; } //获取该Transform对象的平移
-
tf2::Vector3 & getOrigin() const;
-
void setOrigin(const tf2::Vector3& origin)
-
tf2::Quaternion & getRotation() const; //返回旋转对应的四元数
-
void setRotation(const tf2::Quaternion& q)
-
tf2::Transform::getIdentity() // 返回一个单位tf变换
-
void setIdentity() //将一个Transform对象设置为单位变换（旋转矩阵为单位矩阵，平移向量为零向量）
tf2::convert
上述代码中，将一个geometry_msgs/Pose转换为一个transform对象
// 该方法可以实现很多类型的转换，比如
tf2::convert(latest_tf_.inverse(), tmp_tf_stamped.transform); // 将一个transform对象转换为geometry_msgs::TransformStamped中的transform
tf2::convert(msg.pose.pose, pose_old); // 两个位姿geometry_msgs::Pose中的pose之间的变换
tf2::convert(latest_odom_pose_.pose, odom_pose_tf2); // pose与tf2::Transform之间的转换
tf2::Quaternion q;
geometry_msgs::QuaternionStamped min_q, inc_q;
tf2::convert(q, inc_q.quaternion); // 四元数的转换
transform 重载 *
TF2SIMD_FORCE_INLINE Transform
Transform::operator*(const Transform& t) const
{
return Transform(m_basis * t.m_basis, (*this)(t.m_origin));
// 调用了构造函数，传参：旋转和平移
// (*this)(t.m_origin)这是使用了()运算符，参考下面解释
}
tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2; AMCL代码中的这一句代码什么含义？
功能：tf1,tf2是两个tf2::Transform对象；
tf1*tf2 得到一个新的tf2::Transform，其中得到的旋转矩阵为：tf1.旋转矩阵 * tf2.旋转矩阵
- 此处的乘积的顺序，对于计算结果是有影响的；
比如有3个坐标系：map，base1，base2
tf1 : base1->map的坐标变换
tf2：map->base2的坐标变换
故，需要按照左乘实现连续的坐标变换，tf2*tf1实现了base1 -> base2的坐标变换；
transform 重载 ()
/**@brief Return the transform of the vector */
TF2SIMD_FORCE_INLINE Vector3 operator()(const Vector3& x) const
{
return Vector3(m_basis[0].dot(x) + m_origin.x(),
m_basis[1].dot(x) + m_origin.y(),
m_basis[2].dot(x) + m_origin.z());
}
功能：Transform对象包含：旋转和平移；
也就是T12（R12+t12），即R12= 坐标系2相对于坐标系1的旋转矩阵；t12=坐标系2在坐标系1中的坐标；
所以上述重载()的功能就是：tfxxx(vec)，是将坐标系2中的某个向量vec变换到坐标系1中；
四元数消息与四元数对象转换
#include <tf2_geometry_msgs/tf2_geometry_msgs.h> // 提供了tf数据类型的转换
tf2::Quaternion q_tf; // tf2的四元数对象
geometry_msgs::Quaternion q_msg; // ros的四元素消息
/* 消息和类型转换 */
// msg -> Quaternion对象
tf2::convert(q_msg, q_tf); //①msg类型转换为tf类型
tf2::fromMsg(q_msg, q_tf); //②msg类型转换为tf类型
// Quaternion对象 -> 消息
q_msg = tf2::toMsg(q_tf); //tf类型转换为msg类型
- 注意头文件："tf2_geometry_msgs/tf2_geometry_msgs.h"，用于四元数对象和消息做转换
tf2广播
#include <tf2_ros/transform_broadcaster.h> // tf2_ros 广播的头文件
#include <geometry_msgs/TransformStamped.h> // tf消息类型
tf2_ros::TransformBroadcaster br; // 定义一个广播器
geometry_msgs::TransformStamped transformStamped; // 定义一个消息类型
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world"; // 父坐标系
transformStamped.child_frame_id = turtle_name; // child坐标系
// 坐标的平移
transformStamped.transform.translation.x = msg->x;
transformStamped.transform.translation.y = msg->y;
transformStamped.transform.translation.z = 0.0;
tf2::Quaternion q;
q.setRPY(0, 0, msg->theta);//RPY转成tf格式的四元素
// 坐标的旋转
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
br.sendTransform(transformStamped);//sendTransform()
launch中的静态tf发布（广播）
// Usage: static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period(milliseconds)
OR
// Usage: static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period(milliseconds)
<launch>
<node pkg="tf" type="static_transform_publisher" name="map_to_base1" args="0 1 0 0 0 0 /map /base1 10"/>
<node pkg="tf" type="static_transform_publisher" name="map_to_base1" args="0 1 0 0 0 0 /map /base1 10"/>
</launch>
static_transform_publisher 的使用语法
// 旋转是通过ypr表示的；
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
Publish a static coordinate transform to tf using an x/y/z offset in meters and yaw/pitch/roll in radians. (yaw is rotation about Z, pitch is rotation about Y, and roll is rotation about X). The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value.
// 旋转是通过四元数:x,y,z,w表示的
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
Publish a static coordinate transform to tf using an x/y/z offset in meters and quaternion. The period, in milliseconds, specifies how often to send a transform. 100ms (10hz) is a good value.
## 后面的四元数是归一化之后的；
<node pkg="tf" type="static_transform_publisher" name="map_to_base1" args="0.3 0.1 0.1 0.39036 0.58554 0.19518 0.68313 /map /base1 10"/>
## 后面的四元数没有归一化
<node pkg="tf" type="static_transform_publisher" name="map_to_basetest" args="0.3 0.1 0.1 0.2 0.3 0.1 0.35 /map /basetest 10"/>
说明：四元素的归一化在“static_transform_publisher”中没有影响；
tf2监听
#include <tf2_ros/transform_listener.h> //与tf监听有些区别
#include <geometry_msgs/TransformStamped.h> //与tf监听有些区别
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
while(ros::ok()){
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0), ros::Duration(3.0));
//lookupTransform(“target frame”, “source frame”,ros::Time(0));第三个参数ros::Time(0)代表从buffer中获取“最新可获取时间”的变换。
// 每一个listener都有一个buffer储存来自来自不同tf2广播者的坐标系变换关系。这些变换进入缓冲区需要一段很小的时间，所以第三个参数不应该为ros::Time::now(),一般使用ros::Time(0)就很好。
geometry_msgs::PointStamped world, velo_link;
tfBuffer.transform<geometry_msgs::PointStamped>(world, velo_link, "velo_link", ros::Duration(1.0));//transform
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
}
tf2_ros::Buffer
tf2_ros::Buffer 在使用的时候要配合tf2_ros::TransformListener 绑定了后使用；
监听到的tf变换保存在Buffer中，
在Buffer中，可以查找坐标变换，或者把某个位姿变换到不同的坐标系下；
tf2_ros::Buffer可以监听坐标变换，执行变换等
tf2_ros::Buffer的如下几个方法，在amcl代码中使用，其意义理解：
// tf2_ros::Buffer 的几个方法
this->tf_->transform(ident, laser_pose, base_frame_id_);
tf_->transform(min_q, min_q, base_frame_id_);
// handleInitialPoseMessage()函数内部的tf_ buffer的方法
// wait a little for the latest tf to become available
tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,
base_frame_id_, ros::Time::now(),
odom_frame_id_, ros::Duration(0.5));
tf_->setTransform(tf_msg->transforms[ii], "rosbag_authority");
如下参考：tf2_ros::Buffer api，【涉及lookuptransform() transform()内容】
buffer.transform(): 将某个位姿变换到指定的坐标系下
tf2_ros::Buffer 的transform()方法有6个重载，参考上述链接查询，如下是对amcl代码中一种使用方法
try{
tfBuffer.transform(srcpose, targetpose, "map", ros::Duration(1));
std::cout << tf2::getFrameId(targetpose) << endl;
std::cout << tf2::getTimestamp(targetpose) << endl;
std::cout << "position : "
<< targetpose.pose.position.x << " "
<< targetpose.pose.position.y << " "
<< targetpose.pose.position.z << " " << endl;
std::cout << "orientation : "
<< targetpose.pose.orientation.x << " "
<< targetpose.pose.orientation.y << " "
<< targetpose.pose.orientation.z << " "
<< targetpose.pose.orientation.w << " " << endl<< endl;
}
-
tfBuffer.transform(srcpose, targetpose, "map", ros::Duration(1));
-
srcpose，targetpose 必须是带Stamped的pose消息，要使用header中的frame_id，即要知道该pose是在哪个坐标下的；
-
tfBuffer.transform() 的"参数3"是要将"参数1"中的坐标进行不同坐标系下的转换，比如srcpose.header.frame_id = "base1"，所以就是将这个坐标转换到map坐标系下的坐标，将转换后的位姿保存到targetpose，当然也可以是其他类型，内部是通过convert实现不同类型的数据的转换的；
-
tfBuffer.transform() 的“参数4”是查找tf变换的时候，要等待的超时时间，如果不加，当两个时间戳有差异的时候，就会报错。
-
buffer.lookuptransform()：查找两个坐标系之间的变换关系
含有4个参数的基础api
// 01
virtual geometry_msgs::TransformStamped
lookupTransform (const std::string &target_frame,
const std::string &source_frame,
const ros::Time &time,
const ros::Duration timeout) const =0
// Get the transform between two frames by frame ID.
将【参数2：source_frame】中的一个坐标点，变换到【参数1：target_frame】中；
含有6个参数的高级api
// 02
virtual geometry_msgs::TransformStamped
lookupTransform (const std::string &target_frame, const ros::Time &target_time,
const std::string &source_frame, const ros::Time &source_time,
const std::string &fixed_frame, const ros::Duration timeout) const =0
// Get the transform between two frames by frame ID assuming fixed frame.
接口说明：
- 参数3是子坐标系，参数1是坐标系，最终返回的坐标变换 是(参数3@参数1 的坐标关系)
- 该接口，还具有转换不同时间下的坐标变换的功能，这样可以得到在两个不同的时间点上，坐标系之间的差异；
// 5秒前乌龟1的位置，相对与当前乌龟2的位置呢。
try{
ros::Time now = ros::Time::now();
ros::Time past = now - ros::Duration(5.0);
transformStamped = tfBuffer.lookupTransform("turtle2", now, // 当前时刻的turtle2
"turtle1", past, // 5秒前turtle1
"world", ros::Duration(1.0));
}
tf2命名空间中的方法
tf2::getFrameId(msgStamp)
获取消息的坐标系
tf2::getTimestamp(msgStamp);
获取消息的时间戳
geometry_msgs::PoseStamped pstamp;
pstamp.header.stamp = ros::Time::now();
pstamp.header.frame_id = "base1";
std::string frame = tf2::getFrameId(pstamp); // 获取frame_id
ros::Time st1 = tf2::getTimestamp(pstamp); // 获取时间戳
cout << frame<<endl;
cout << int(st1.toSec())<<endl;
tf2::getYaw()
#include <tf2/utils.h>
double deg_yaw = tf2::getYaw(latest_odom_pose_.orientation); // geometry_msgs::Pose中的四元数，提取yaw角
std::cout << "msg-> double " <<deg_yaw<<endl;
deg_yaw = tf2::getYaw(q3); // 从四元数对象tf2::Quaternion，提取yaw角
std::cout << "quaternion-> double " <<deg_yaw<<endl;
