第四章：TF坐标变换
本文的目的是，将所有ROS下与tf坐标变换相关的问题一网打尽。
在代码实践部分，当前ros官方在推tf2，未来的tf要被废弃。所以我们的cpp实现，都是基于tf2编写的。
关于与时间相关的TF变换内容，我们放在了：ROS基础(五)：ROS中的时间与TF中的时间
中的第二部分。有兴趣的朋友可以查看。
一、概述
机器人身体上每个自由度的关节都是一个单独的坐标系。除此之外，机器人移动过程中，本体与世界坐标系之间也有相对位姿变换。
上述所有与机器人坐标变换相关的事情，在ros下都是由TF负责。
三、初识TF：乌龟例程
开启两个终端，分别输入如下内容：
第一个终端输入如下内容，开启乌龟仿真界面。
roslaunch turtle_tf turtle_tf_demo.launch
第二个终端输入：rosrun turtlesim turtle_teleop_key来控制其中第一个小乌龟，然后观察现象。
可以看到，第二个乌龟一直follow第一个乌龟。
具体原理就是第二个乌龟一直通过tf订阅第一个乌龟的位置，然后将位置转换为控制指令，控制自己向第一个乌龟当前的位置移动。
二、TF相关工具
1. rqt_tf_tree
这个工具很有用，实时显示当前所有的tf连接，并以树的形式展示出来。
还是小乌龟案例，终端输入：rosrun rqt_tf_tree rqt_tf_tree，我们可以形象的看到他们之间的tf连接关系。
另外还有一个相似的命令：rosrun tf view_frames, 这个命令可以监听5s的tf_tree，然后将其保存成为pdf文档。没啥用，通常都用上一个命令查看tf_tree是否联通。
2. tf_echo
这条命令可以查看指定两个frame坐标系之间的tf变换。格式如下：
rosrun tf tf_echo <target_frame> <source_frame>
命令查询的是source to target 的坐标变换。也就是，在target的坐标系下，source_frame的坐标位置。
我们用小乌龟案例实操下。终端键入命令rosrun tf tf_echo turtle1 turtle2
我们先来预测下，上述命令查询的是相对于target坐标系turtle1而言，turtle2的相对位置。因为turtle2一直在跟着turtle1走，所以当tuetle1沿着某个方向前进的时候，对应的translation应该只有x轴在变，而且是负数。rotation应该是不变的。我们测试下，是否预测成功。
没问题！！
3. tf_monitor
这个工具可以查看tf_tree的发布状态。我感觉没啥大用。简单说下相关命令：
rosrun tf tf_monitor：监控所有的坐标系发布状态
rosrun tf tf_monitor <frame1> <frame2>： 监控指定两个frame之间的发布状态
4. static_transform_publisher
这个工具发布两个frame之间的静态坐标变换，经常用于发布机器人本身传感器或关节之间的刚体变换。
可以在终端和launch文件中使用，通常出现的场景是在launch文件中。
终端使用
static_transform_publisher x y z yaw pitch roll frame_id child_frame_id
static_transform_publisher x y z qx qy qz qw frame_id child_frame_id
举例：
rosrun tf static_transform_publisher 1 2 1 1 3 4 turtle2 turtle4 1
参数解释：
1 2 1 1 3 4：偏移参数 + 欧拉角，如果是七个数字的话，就是偏移参数 + 四元数
turtle2: father_father_id
turtle4: child_frame_id
1: 发布的transform的时间间隔，1ms发布一次turtle4->turtle2的transform
注意：
上述命令发布的是/turtle4相对于/turtle2 的坐标变换，也就是在/turtle2 坐标系下，/turtle4的位置。按照我们之前的定义，就可以将/turtle2 称为target_frame, 将/turtle4称为source_frame
运行之后的效果：
launch文件中的使用
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="link1_broadcaster" args="1 0 0 0 0 0 1 link1_parent link1" />
</launch>
具体解释同上。
自己编写ros节点来发布静态TF
- 撰写node节点：
//
// Created by xu on 2021/1/14.
//
#include <ros/ros.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <cstdio>
#include <tf2/LinearMath/Quaternion.h>
std::string static_turtle_name;
int main(int argc, char **argv)
{
ros::init(argc,argv, "my_static_tf2_broadcaster");
if(argc != 8)
{
ROS_ERROR("Invalid number of parameters\nusage: static_turtle_tf2_broadcaster child_frame_name x y z roll pitch yaw");
return -1;
}
if(strcmp(argv[1],"world")==0)
{
ROS_ERROR("Your static turtle name cannot be 'world'");
return -1;
}
static_turtle_name = argv[1];
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
geometry_msgs::TransformStamped static_transformStamped;
static_transformStamped.header.stamp = ros::Time::now();
static_transformStamped.header.frame_id = "world";
static_transformStamped.child_frame_id = static_turtle_name;
static_transformStamped.transform.translation.x = atof(argv[2]);
static_transformStamped.transform.translation.y = atof(argv[3]);
static_transformStamped.transform.translation.z = atof(argv[4]);
tf2::Quaternion quat;
quat.setRPY(atof(argv[5]), atof(argv[6]), atof(argv[7]));
static_transformStamped.transform.rotation.x = quat.x();
static_transformStamped.transform.rotation.y = quat.y();
static_transformStamped.transform.rotation.z = quat.z();
static_transformStamped.transform.rotation.w = quat.w();
static_broadcaster.sendTransform(static_transformStamped);
ROS_INFO("Spinning until killed publishing %s to world", static_turtle_name.c_str());
ros::spin();
return 0;
};
- 修改CMakeLists
add_executable(static_turtle_tf2_broadcaster src/static_turtle_tf2_broadcaster.cpp)
target_link_libraries(static_turtle_tf2_broadcaster ${catkin_LIBRARIES} )
-
工作空间
catkin_make编译 -
终端运行：
rosrun learn_tf2 static_turtle_tf2_broadcaster myworld 1 2 3 4 5 6 -
查看运行结果：
5.tf in rviz
同样，万能的可视化工具rviz也有tf可视化插件
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
启动rviz，然后控制turtle1运动，在rviz中查看两龟共舞。
四、TF的API（C++）
1. 对应头文件
#include <tf2_ros/transform_broadcaster.h>
2. 创建TransformBroadcaster对象
static tf2_ros::TransformBroadcaster br;
3. 创建要传输的对象并填充内容
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = ros::Time::now();
transformStamped.header.frame_id = "world";
transformStamped.child_frame_id = turtle_name;
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);
transformStamped.transform.rotation.x = q.x();
transformStamped.transform.rotation.y = q.y();
transformStamped.transform.rotation.z = q.z();
transformStamped.transform.rotation.w = q.w();
4. 发射
br.sendTransform(transformStamped);
五、手写小乌龟follow实例
首先建立tf_tree: 用一个tf_boardcaster分别发布两个结点相对于world坐标系下的tf
#include <ros/ros.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg){
static 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;
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);
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);
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_tf2_broadcaster");
// parameter server or use args
ros::NodeHandle private_node("~");
if (! private_node.hasParam("turtle"))
{
if (argc != 2){
ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
}
else
{
private_node.getParam("turtle", turtle_name);
}
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
}
然后建立一个tf_listener， 监听/turtle1 相对于/turtle2的位置，并将之转换为twist
//
// Created by xu on 2021/1/15.
//
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char** argv){
ros::init(argc, argv, "follower");
// client
ros::NodeHandle nh("~");
ros::service::waitForService("/spawn");
ros::ServiceClient turtle_client = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 2;
spawn.request.y = 2;
turtle_client.call(spawn);
ros::Publisher turtle2_pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
double scale_linear, scale_angular;
nh.param("scale_linear", scale_linear, 2.0);
nh.param("scale_angular", scale_angular, 2.0);
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10);
while (ros::ok()){
geometry_msgs::Twist cmd_vel;
// tf2 listener: turtle2 to turtle1
geometry_msgs::TransformStamped transformStamped, transformStamped1;
try{
// T_turtle2_turtle1
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time(0));
}catch (tf2::TransformException &exception){
ROS_WARN("wait tf tree: %s",exception.what());
ros::Duration(1.0).sleep();
continue;
}
cmd_vel.angular.z = 4 * atan2(transformStamped.transform.translation.y,
transformStamped.transform.translation.x);
cmd_vel.linear.x = 0.5 * sqrt(pow(transformStamped.transform.translation.x, 2) +
pow(transformStamped.transform.translation.y, 2));
turtle2_pub.publish(cmd_vel);
rate.sleep();
}
return 0;
}
最后撰写launch文件，将以上所有节点都启动起来：
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- <node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>-->
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<node pkg="learn_tf2" type="turtle_tf2_broadcaster"
args="/turtle1" name="turtle1_tf2_broadcaster" />
<node pkg="learn_tf2" type="turtle_tf2_broadcaster"
args="/turtle2" name="turtle2_tf2_broadcaster" />
<node pkg="learn_tf2" type="follower" name="turtle_tf2_listener" output="screen"/>
</launch>