ROS学习笔记（四）
TF坐标变换
tf2是转换库的第二代，它允许用户随时间跟踪多个坐标帧。tf2在时间缓冲的树结构中维护坐标帧之间的关系，并允许用户在任何所需的时间点在任意两个坐标帧之间转换点、向量等。
坐标msgs消息
在坐标转换实现中常用的
msg:geometry_msgs/TransformStamped和geometry_msgs/PointStamped
前者用于传输坐标系相关位置信息，后者用于传输某个坐标系内坐标点的信息。在坐标变换中，频繁的需要使用到坐标系的相对关系以及坐标点信息。
静态坐标变换
是指两个坐标系之间的相对位置是固定的
创建项目功能包依赖于 tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
发布方代码
#include "ros/ros.h"
#include "tf2_ros/static_transform_broadcaster.h"//发布对象头文件，静态坐标广播器
#include "geometry_msgs/TransformStamped.h"//用于传输坐标系相关位置信息
#include "tf2/LinearMath/Quaternion.h"
/*
需求：发布两个坐标系的相对关系
流程：
1.包含头文件
2.设置编码 节点初始化 NodeHandle
3.创建发布者消息
4.组织被发布的消息
5.发布数据
6.spin()
*/
int main(int argc, char *argv[])
{
// 1.包含头文件
// 2.设置编码 节点初始化 NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_pub");
ros::NodeHandle nh;
// 3.创建发布者消息，，静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster pub;
// 4.组织被发布的消息，，创建坐标信息
geometry_msgs::TransformStamped tfs;
tfs.header.stamp = ros::Time::now();//获取当前时刻
tfs.header.frame_id = "base_link";//相对坐标系中被参考的那一个
tfs.child_frame_id = "laser";
//设置子级相对于父级的偏移量(translation)
tfs.transform.translation.x = 0.2;
tfs.transform.translation.y = 0;
tfs.transform.translation.z = 0.5;
//需要根据欧辣角转换
tf2::Quaternion qtn; //创建 四元数(rotation) 对象
//向该对象设置欧拉角，这个对象可以将欧拉角转换成四元数
qtn.setRPY(0,0,0);//欧拉角的单位是弧度，如果雷达装反了，设置成3.14
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 5.发布数据
pub.sendTransform(tfs);
// 6.spin()
ros::spin();
return 0;
}
订阅方代码
#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" //注意: 调用 transform 必须包含该头文件
/*
流程：
1.包含头文件
2.编码、初始化、NodeHandle
3.创建订阅者对象------>订阅坐标系相对关系
4.组织一个坐标点数据
5.转换算法，需要调用TF内置实现
6.最后输出
*/
int main(int argc, char *argv[])
{
// 2.编码、初始化、NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"static_sub");
ros::NodeHandle nh;
// 3.创建订阅者对象------>订阅坐标系相对关系
//3-1 创建一个 buffer 缓存
tf2_ros::Buffer buffer;
//3-2 再创建监听对象（监听对象可以将订阅的数据存入buffer）
tf2_ros::TransformListener listener(buffer);
// 4.组织一个坐标点数据
geometry_msgs::PointStamped ps;
ps.header.frame_id = "laser";
ps.header.stamp = ros::Time::now();//时间戳
//设置障碍物坐标
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 5.0;
//添加休眠，防止接受不到信息
//ros::Duration(2).sleep();
// 5.转换算法，需要调用TF内置实现
ros::Rate rate(10);
while (ros::ok())
{
//将 ps 转换成相对于 base_link 的坐标点
geometry_msgs::PointStamped ps_out;
/*
调用了 buffer 的转换函数 transform
参数1：被转换的坐标点
参数2.目标坐标系
返回值：输出的坐标点
PS1: 调用 transform 必须包含该头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
PS2：运行时存在的问题：抛出一个异常 base_link 不存在
原因：订阅数据是一个耗时操作，可能再调用 transform 转换函数时，坐标系的相对关系 还没有订阅到，因此异常。
解决1：在调用转换函数前，执行休眠
2：进行异常处理 try
*/
try
{
ps_out = buffer.transform(ps,"base_link");
// 6.最后输出
ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
rate.sleep();
ros::spinOnce();
}
catch(const std::exception& e)
{
//std::cerr << e.what() << '\n';
ROS_INFO("异常消息:%s",e.what());
}
}
return 0;
}
补充1:
当坐标系之间的相对位置固定时，那么所需参数也是固定的: 父系坐标名称、子级坐标系名称、x偏移量、y偏移量、z偏移量、x 翻滚角度、y俯仰角度、z偏航角度，实现逻辑相同，参数不同，那么 ROS 系统就已经封装好了专门的节点，使用方式如下:
rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系
示例:rosrun tf2_ros static_transform_publisher 0.2 0 0.5 0 0 0 /baselink /laser
也建议使用该种方式直接实现静态坐标系相对信息发布。
补充2:
可以借助于rviz显示坐标系关系，具体操作:
- 新建窗口输入命令:rviz;
- 在启动的 rviz 中设置Fixed Frame 为 base_link;
- 点击左下的 add 按钮，在弹出的窗口中选择 TF 组件，即可显示坐标关系。
动态坐标变换
所谓动态坐标变换，是指两个坐标系之间的相对位置是变化的。
需求描述:
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
实现分析:
- 乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
- 订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
- 将 pose 信息转换成 坐标系相对信息并发布
实现流程:
- 新建功能包，添加依赖tf2 tf2_ros tf2_geometry_msgs roscpp rospy std_msgs geometry_msgs turtlesim
- 创建坐标相对关系发布方(同时需要订阅乌龟位姿信息)
- 创建坐标相对关系订阅方
- 执行
编写发布方程序
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h" //动态发布
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
/*
发布方：需要订阅乌龟的位姿信息，转换成相对于窗体的坐标关系，并发布
准备：
话题：/turtle1/pose
消息：/turtle1/Pose
流程：
1.包含头文件
2.设置编码、初始化、句柄。
3.创建订阅对象，订阅 /turtle1/pose
4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布
5.回旋spin()
*/
//const 只读，确保引用的数据不被修改
void doPose(const turtlesim::Pose::ConstPtr& pose){
//获取位姿信息，转换成坐标系相对关系(核心)，并发布
//a.创建发布对象
static tf2_ros::TransformBroadcaster pub;//使用static关键字,变成静态的。每次回调都使用同一个pub对象
//b.组织被发布的数据
geometry_msgs::TransformStamped ts;
ts.header.frame_id = "world";
ts.header.stamp = ros::Time::now();
ts.child_frame_id = "turtle1";
//坐标偏移量设置，2D 所以z是0
ts.transform.translation.x = pose->x;
ts.transform.translation.y = pose->y;
ts.transform.translation.z = 0;
/*
位姿信息中没有四元数，但是有个偏航角度，又已知乌龟是 2D 的，没有翻滚与俯仰角度，所以得出乌龟的欧拉角：0 0 theta
*/
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();
//c.发布
pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
// 2.设置编码、初始化、句柄。
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_pub");
ros::NodeHandle nh;
// 3.创建订阅对象，订阅 /turtle1/pose
ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,doPose);
// 4.回调函数处理订阅的消息：将位姿信息转换成坐标相对关系并发布
// 5.回旋spin()
ros::spin();
return 0;
}
roscore
启动乌龟节点 rosrun turtlesim turtlesim_node
启动键盘控制节点 rosrun turtlesim turtle_teleop_key
启动文件 source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub（注意回旋函数）
rostopic echo /tf（移动乌龟数据会有变化）
编写订阅方代码（除参考的坐标系，目标坐标系，时间戳以外，和静态坐标转换相同）
#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" //注意: 调用 transform 必须包含该头文件
/*
流程：
1.包含头文件
2.编码、初始化、NodeHandle
3.创建订阅者对象------>订阅坐标系相对关系
4.组织一个坐标点数据
5.转换算法，需要调用TF内置实现
6.最后输出
*/
int main(int argc, char *argv[])
{
// 2.编码、初始化、NodeHandle
setlocale(LC_ALL,"");
ros::init(argc,argv,"dynamic_sub");
ros::NodeHandle nh;
// 3.创建订阅者对象------>订阅坐标系相对关系
//3-1 创建一个 buffer 缓存
tf2_ros::Buffer buffer;
//3-2 再创建监听对象（监听对象可以将订阅的数据存入buffer）
tf2_ros::TransformListener listener(buffer);
// 4.组织一个坐标点数据
geometry_msgs::PointStamped ps;
//参考的坐标系
ps.header.frame_id = "turtle1";
ps.header.stamp = ros::Time(0.0);//时间戳
//设置障碍物坐标
ps.point.x = 2.0;
ps.point.y = 3.0;
ps.point.z = 0;
//添加休眠，防止接受不到信息
//ros::Duration(2).sleep();
// 5.转换算法，需要调用TF内置实现
ros::Rate rate(10);
while (ros::ok())
{
//将 ps 转换成相对于 base_link 的坐标点
geometry_msgs::PointStamped ps_out;
/*
调用了 buffer 的转换函数 transform
参数1：被转换的坐标点
参数2.目标坐标系
返回值：输出的坐标点
PS1: 调用 transform 必须包含该头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
PS2：运行时存在的问题：抛出一个异常 base_link 不存在
原因：订阅数据是一个耗时操作，可能再调用 transform 转换函数时，坐标系的相对关系 还没有订阅到，因此异常。
解决1：在调用转换函数前，执行休眠
2：进行异常处理 try
*/
try
{
ps_out = buffer.transform(ps,"world");
// 6.最后输出
ROS_INFO("转换后的坐标值:(%.2f,%.2f,%.2f),参考的坐标系:%s",
ps_out.point.x,
ps_out.point.y,
ps_out.point.z,
ps_out.header.frame_id.c_str());
rate.sleep();
ros::spinOnce();
}
catch(const std::exception& e)
{
//std::cerr << e.what() << '\n';
ROS_INFO("异常消息:%s",e.what());
}
}
return 0;
}
roscore
启动乌龟节点 rosrun turtlesim turtlesim_node
启动文件 source ./devel/setup.bash
rosrun tf02_dynamic demo01_dynamic_pub（注意回旋函数）
新建窗口 source ./devel/setup.bash
rosrun tf02_dynamic demo02_dynamic_sub
启动键盘控制节点 rosrun turtlesim turtle_teleop_key
多坐标变换
需求描述:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现分析:
- 首先，需要发布 son1 相对于 world，以及 son2 相对于 world 的坐标消息
- 然后，需要订阅坐标发布消息，并取出订阅的消息，借助于 tf2 实现 son1 和 son2 的转换
- 最后，还要实现坐标点的转换
打开launch文件
打开订阅方文件
#include "ros/ros.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
/*
订阅方实现， 1.计算son1与son2的相对关系
2.计算son1中某坐标点在 son2 中的坐标值
*/
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"tfs_sub");
ros::NodeHandle nh;
//创建订阅者对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
//创建坐标点
geometry_msgs::PointStamped psAtSon1;
psAtSon1.header.stamp = ros::Time::now();
psAtSon1.header.frame_id = "son1";
psAtSon1.point.x = 1.0;
psAtSon1.point.y = 2.0;
psAtSon1.point.z = 3.0;
//编写解析逻辑
ros::Rate rate(10);
while (ros::ok())
{
try
{
//1.计算son1与son2的相对关系
/*
A 相对于 B 的坐标系关系
参数1：目标坐标系 B
参数2：源坐标系 A
参数3.ros::Time(0) 取间隔最短的两个坐标系关系帧计算相应关系
返回值：gemetry_msgs::TransformStamped 源相对于目标坐标系的相对关系
*/
geometry_msgs::TransformStamped son1Toson2 = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("son1 相对于 son2 的信息：父级:%s 子级:%s 偏移量(%.2f,%.2f,%.2f)",
son1Toson2.header.frame_id.c_str(),
son1Toson2.child_frame_id.c_str(),
son1Toson2.transform.translation.x,
son1Toson2.transform.translation.y,
son1Toson2.transform.translation.z
);
//2.计算son1中的某个坐标点在 son2 中的坐标值
geometry_msgs::PointStamped psAtSon2 = buffer.transform(psAtSon1,"son2");
ROS_INFO("坐标点在 Son2 中的值(%.2f,%.2f,%.2f)",
psAtSon2.point.x,
psAtSon2.point.y,
psAtSon2.point.z
);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
坐标关系
sudo apt install ros-noetic-tf2-tools
rosrun tf2_tools view_frames.py
坐标变换实操
控制乌龟运动，编写乌龟启动launch文件
启动launch文件
启动第二只乌龟运动
rostopic list
rostopic pub -r 10 /turtle2/cmd_vel geometry_msgs/Twist
修改参数
服务客户端（生成乌龟）
/*
创建第二只小乌龟
*/
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
//执行初始化
ros::init(argc,argv,"create_turtle");
//创建节点
ros::NodeHandle nh;
//创建服务客户端
ros::ServiceClient client = nh.serviceClient("/spawn");
ros::service::waitForService("/spawn");
turtlesim::Spawn spawn;
spawn.request.name = "turtle2";
spawn.request.x = 1.0;
spawn.request.y = 2.0;
spawn.request.theta = 3.12415926;
bool flag = client.call(spawn);
if (flag)
{
ROS_INFO("乌龟%s创建成功!",spawn.response.name.c_str());
}
else
{
ROS_INFO("乌龟2创建失败!");
}
ros::spin();
return 0;
}
发布两只乌龟信息
- 该节点需要启动两次
- 每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)
/*
该文件实现:需要订阅 turtle1 和 turtle2 的 pose，然后广播相对 world 的坐标系信息
注意: 订阅的两只 turtle,除了命名空间(turtle1 和 turtle2)不同外,
其他的话题名称和实现逻辑都是一样的，
所以我们可以将所需的命名空间通过 args 动态传入
实现流程:
1.包含头文件
2.初始化 ros 节点
3.解析传入的命名空间
4.创建 ros 句柄
5.创建订阅对象
6.回调函数处理订阅的 pose 信息
6-1.创建 TF 广播器
6-2.将 pose 信息转换成 TransFormStamped
6-3.发布
7.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"
#include "geometry_msgs/TransformStamped.h"
//保存乌龟名称
std::string turtle_name;
void doPose(const turtlesim::Pose::ConstPtr& pose){
// 6-1.创建 TF 广播器 ---------------------------------------- 注意 static
static tf2_ros::TransformBroadcaster broadcaster;
// 6-2.将 pose 信息转换成 TransFormStamped
geometry_msgs::TransformStamped tfs;
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
tfs.child_frame_id = turtle_name;
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0;
tf2::Quaternion qtn;
qtn.setRPY(0,0,pose->theta);
tfs.transform.rotation.x = qtn.getX();
tfs.transform.rotation.y = qtn.getY();
tfs.transform.rotation.z = qtn.getZ();
tfs.transform.rotation.w = qtn.getW();
// 6-3.发布
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"pub_tf");
// 3.解析传入的命名空间
if (argc != 2)
{
ROS_ERROR("请传入正确的参数");
} else {
turtle_name = argv[1];
ROS_INFO("乌龟 %s 坐标发送启动",turtle_name.c_str());
}
// 4.创建 ros 句柄
ros::NodeHandle nh;
// 5.创建订阅对象
ros::Subscriber sub = nh.subscribe(turtle_name + "/pose",1000,doPose);
// 6.回调函数处理订阅的 pose 信息
// 6-1.创建 TF 广播器
// 6-2.将 pose 信息转换成 TransFormStamped
// 6-3.发布
// 7.spin
ros::spin();
return 0;
}
订阅方，解析坐标信息并生成速度信息
/*
订阅 turtle1 和 turtle2 的 TF 广播信息，查找并转换时间最近的 TF 信息
将 turtle1 转换成相对 turtle2 的坐标，在计算线速度和角速度并发布
实现流程:
1.包含头文件
2.初始化 ros 节点
3.创建 ros 句柄
4.创建 TF 订阅对象
5.处理订阅到的 TF
6.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"sub_TF");
// 3.创建 ros 句柄
ros::NodeHandle nh;
// 4.创建 TF 订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
// 5.处理订阅到的 TF
// 需要创建发布 /turtle2/cmd_vel 的 publisher 对象
ros::Publisher pub = nh.advertise("/turtle2/cmd_vel",1000);
ros::Rate rate(10);
while (ros::ok())
{
try
{
//5-1.先获取 turtle1 相对 turtle2 的坐标信息
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
//5-2.根据坐标信息生成速度信息 -- geometry_msgs/Twist.h
geometry_msgs::Twist twist;
twist.linear.x = 0.5 * sqrt(pow(tfs.transform.translation.x,2) + pow(tfs.transform.translation.y,2));
twist.angular.z = 4 * atan2(tfs.transform.translation.y,tfs.transform.translation.x);
//5-3.发布速度信息 -- 需要提前创建 publish 对象
pub.publish(twist);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
运行launch文件