目录
- 一、静态坐标变换（C++实现）
- 二、静态坐标变换（Python实现）
如前文所属，ROS通过广播的形式告知各模块的位姿关系，接下来详述这一机制的代码实现。
模块间的位置关系有两种类型，一种是相对固定的，称为静态坐标变换，一种是相对不固定，变化的，称为动态坐标变换。
一、静态坐标变换（C++实现）
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。比如机器人底盘上安装了一个激光雷达，他和底盘组成一个刚体，它们的相对位姿不会随机器人的运动而变化，他们之间的坐标变换即属于静态坐标变换。
假设激光雷达相对与底盘的欧拉位姿为（0.5, 0.0, 0.3; 0.0, 0.0, 0.0）
雷达检测到的障碍物位置为（2.0, 2.5, 0.3）
若要计算障碍物和底盘的相对位置，就可以通过雷达到底盘的坐标变换来计算，步骤如下：
- 雷达（laser）发布自己和底盘（base_link）的相对静态坐标
- 避障模块监听雷达（laser）和底盘（base_link）的相对坐标关系，并通过
tf计算障碍物位置。
首先创建 tf2_learning 包，命令如下：（这一步不是必须，这里只是为了方便清晰的说明，也可以使用已有的包，在包里新增节点等方法）
catkin_creat_pkg tf2_learning roscpp rospy geometry_msgs std_msgs tf2 tf2_geometry_msgs tf2_ros
创建后，文件结构如下：
在创建的 tf2_learning 包路径下有一个 src 目录，在这里存储C++源码，我们创建 static_frame_broadcast.cpp 和 static_frame_listen.cpp ，修改 CMakeLists.txt ，添加如下内容：
add_executable(${PROJECT_NAME}_broadcast src/static_frame_broadcast.cpp)
add_executable(${PROJECT_NAME}_listen src/static_frame_listen.cpp)
target_link_libraries(${PROJECT_NAME}_broadcast
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_listen
${catkin_LIBRARIES}
)
static_frame_broadcast.cpp 实现广播子坐标系相对于父坐标系的静态坐标，内容如下：
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
int main(int argc, char **argv)
{
// 初始化 ROS 节点
ros::init(argc, argv, "static_frame_broadcast");
// 创建静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 创建坐标系信息
geometry_msgs::TransformStamped ts;
// --设置头信息
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();
ts.header.frame_id = "base_link";
// --设置子级坐标系
ts.child_frame_id = "laser";
// --设置子坐标系相对于父坐标系的平移偏移量
ts.transform.translation.x = 0.5;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.3;
// --设置子坐标系相对于父坐标系的旋转偏移量
// --将欧拉角转换成四元数
tf2::Quaternion qtn; // tf2的四元数类
qtn.setRPY(0, 0, 0); // 设置欧拉角
// 获取旋转的四元数值
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
// 广播器发布坐标系信息
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}
static_frame_listen.cpp 实现订阅静态坐标转换关系，并利用该关系将雷达坐标系的点转换到 base_link 坐标系，内容如下：
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc, char *argv[])
{
// 初始化 ROS 节点
ros::init(argc, argv, "static_frame_listen");
ros::NodeHandle nh;
// 创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate rate(1);
while (ros::ok())
{
// 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 2.0;
point_laser.point.y = 2.5;
point_laser.point.z = 0.3;
// 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser, "base_link");
ROS_INFO("point_base: (%.2f, %.2f, %.2f), frame: %s",
point_base.point.x, point_base.point.y, point_base.point.z,
point_base.header.frame_id.c_str());
}
catch (const std::exception &e)
{
ROS_ERROR("%s", e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
编译后，执行 rosrun tf2_learning tf2_learning_broadcast 开始广播坐标，此时打开rviz订阅TF看到TF树模型，操作与结果如下：
- 输入命令：rviz
- 在启动的 rviz 中设置
Fixed Frame为base_link - 点击左下的
Add按钮，在弹出的窗口中选择TF组件，即可显示坐标关系。
继续执行命令rosrun tf2_learning tf2_learning_listen可以看到转换后的坐标，以及所属父坐标系，如下：
其中，ERROR是由于节点刚起来时，TF数据还未来得及写入缓存，导致base_link不存在，可以发现第二次调用就没有报错了，实际使用中，可以等待要操作的frame存在再做转换，如下：
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// _frameExists()返回指定frame是否存在于tf树中
if (!buffer._frameExists("base_link"))
{
ROS_WARN("base_link frame does not exist.");
}
二、静态坐标变换（Python实现）
在创建的 tf2_learning 包路径下 src 目录的同级，创建一个 scripts 目录，在这里存储脚本（如python脚本），我们创建 tf2_learning_broadcast.py 以实现坐标广播，编辑内容如下：
#! /usr/bin/env python
import rospy
import tf
import tf2_ros
from geometry_msgs.msg import TransformStamped
if __name__ == "__main__":
# 初始化 ROS 节点
rospy.init_node("static_frame_broadcast_py")
# 创建静态坐标广播器
broadcaster = tf2_ros.StaticTransformBroadcaster()
# 创建并组织被广播的消息
tfs = TransformStamped()
# -- 头信息
tfs.header.frame_id = "base_link" # 父坐标系
tfs.header.stamp = rospy.Time.now()
tfs.header.seq = 101
# -- 子坐标系
tfs.child_frame_id = "laser"
# -- 坐标系相对信息
# ---- 相对于父坐标系的平移偏移量
tfs.transform.translation.x = 0.5
tfs.transform.translation.y = 0.0
tfs.transform.translation.z = 0.3
# ---- 相对于父坐标系的旋转偏移量
# ---- 设置欧拉角，并将欧拉角转换成四元数
qtn = tf.transformations.quaternion_from_euler(0, 0, 0)
tfs.transform.rotation.x = qtn[0]
tfs.transform.rotation.y = qtn[1]
tfs.transform.rotation.z = qtn[2]
tfs.transform.rotation.w = qtn[3]
# 广播器发送消息
broadcaster.sendTransform(tfs)
# spin
rospy.spin()
创建 tf2_learning_listen.py 以订阅静态坐标转换关系，并利用该关系将雷达坐标系的点转换到 base_link 坐标系，编辑内容如下：
#! /usr/bin/env python
import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped
if __name__ == "__main__":
# 初始化 ROS 节点
rospy.init_node("static_frame_listen")
# 创建 TF 订阅对象
buffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buffer)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# 生成一个坐标点, 模拟雷达检测到的障碍物坐标点(雷达坐标系下的坐标)
point_laser = PointStamped()
point_laser.header.frame_id = "laser"
point_laser.header.stamp = rospy.Time.now()
point_laser.point.x = 2.0
point_laser.point.y = 2.5
point_laser.point.z = 0.3
try:
# 转换坐标点, 计算障碍物坐标点在 base_link 下的坐标
point_base = buffer.transform(point_laser, "base_link")
rospy.loginfo("point_base: (%.2f, %.2f, %.2f), frame: %s",
point_base.point.x,
point_base.point.y,
point_base.point.z,
point_base.header.frame_id)
except Exception as e:
rospy.logerr("%s", e)
# spin
rate.sleep()
