ROS学习笔记10：TF坐标变换（ROS常用组件）
前言：
机器人本体和机器人的工作环境中往往存在大量的组件元素，在机器人设计和应用中会涉及不同组件的位置和姿态，这就需要引入坐标系和坐标变换的概念。
一、机器人中空间描述和变换：
1.位置描述：
一旦建立了坐标系，就可以用一个3*1的位置矢量对世界坐标系中的任何点进行定位。由于世界坐标系中通常还要定义许多坐标系，需在位置矢量上附加说明是在哪一个坐标系中定义的。
2.姿态描述：
位置描述只能表示空间的点，但对于末端执行器还需要描述其空间的姿态。
3.坐标系的变换：
二、TF功能包：
1.TF功能包的功能：
TF是一个能让用户随时间跟踪多个坐标系的功能包。它使用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间、坐标系间完成点、向量等坐标的变换。
TF可以请求如下类型的数据：
（1）5秒之前机器人头部坐标系相对于全局坐标系的关系；
（2）机器人夹取的物体相对于机器人中心坐标系的位置；
（3）机器人中心坐标系相对于全局坐标系的位置。
使用TF功能包需要以下两个步骤：
（1）监听TF变换：
接收并缓存系统中发布的所有坐标变换数据，并从中查询作需要的坐标变换关系。
（2）广播TF变换：
向系统中广播坐标系之间的坐标变换关系。
2.TF功能包安装：
sudo apt-get install ros-noetic-turtle-tf
roslaunch turtle_tf turtle_tf_demo.launch
rosrun turtlesim turtle_teleop_keyPS：如果roslaunch turtle_tf turtle_tf_demo.launch报错，运行以下命令：
sudo apt install python-is-python3
3.乌龟例程中的TF：
（1）功能：
键盘控制乌龟1的运动，同时乌龟2会规划最短路径跟随乌龟1。该程序用于检验TF安装是否成功，同时是后续学习TF的广播和监听功能的载体。
（2）TF功能包安装和排错过程：
（3）运行程序：
4.TF工具：
（1）view_frames工具：
可视化系统当中的所有tf的关系，监听5s，把5s之内所有坐标系之间的关系保存下来，并生成一个pdf。
rosrun tf view_framesPS：如果出现以下报错，修改view_frames文件第89行，由于该文件为只读文件，需获取权限后打开。
将第89行的m = r.search(vstr)修改为m = r.search(vstr.decode('utf-8'))
再次运行：
生成的pdf文件位于主目录下：
（2）tf_echo 工具：
查看指定坐标系之间的变换命令。以下代码查询turtle1和turtle2之间的位置关系。
rosrun tf tf_echo turtle1 turtle2
（3）可视化工具 rviz：
1）安装rviz：
sudo apt-get install ros-noetic-rviz
2）使用rviz观察上述乌龟例程：
rosrun rviz rviz -d `rospack find turtle_tf`/rviz/turtle_rviz.rviz
3）添加一个TF坐标：
（4）tf_monitor 工具：
打印TF树中所有坐标系的发布状态，可查询指定坐标系之间的发布状态。
rosrun tf tf_monitor
rosrun tf tf_monitor turtle1 turtle2
（5）static_transform_publisher 工具：
静态发布一个从父坐标系到子坐标系的一个坐标变换。（这个用的非常频繁！）
rosrun tf static_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_msrosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms以上两种命令格式需要设置坐标的偏移参数和旋转参数：偏移参数使用相对于x、y、z三轴的坐标位移；旋转参数的第一种命令格式使用以弧度为单位的yaw、pitch、roll角度（yaw是围绕z轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕x轴旋转的翻滚角），第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位。
该命令不仅可以在终端使用，还可以在launch文件中使用：
1）在launch文件中添加如下片段
2）启动launch文件：
roslaunch turtle_tf turtle_tf_demo.launch
三、TF坐标系广播与监听的编程实现：
1.创建功能包：
2.如何实现一个TF广播器：
（1）定义TF广播器
（2）创建坐标变换值
（3）发布坐标变换
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程产生tf数据，并计算、发布turtle2的速度指令
*/
#include
#include
#include
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster br;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
};
3.如何实现一个TF监听器：
（1）定义TF监听器
（2）查找坐标变换
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程监听tf数据，并计算、发布turtle2的速度指令
*/
#include
#include
#include
#include
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");
// 创建节点句柄
ros::NodeHandle node;
// 请求产生turtle2
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = node.advertise("/turtle2/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系，发布turtle2的速度控制指令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
}; 4.将广播器和监听器文件放在catkin_ws/src/learning_tf/src目录下：
5.配置CMakeLists中的编译规则：
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
6.编译并运行：
下面运行broadcaster，发布两只小海龟和world坐标系之间的位置关系：
