- 坐标变换是机器人系统中常用的基础功能,ROS中坐标变换由TF功能包维护。它可以根据时间缓冲并维护多个坐标系之间的坐标变换关系,目前 tf2 比 tf 用的多。
1. 坐标变换常用msg
1.1 geometry_msgs/TransformStamped–>用于传输坐标系相关位置信息
命令行键入:rosmsg info geometry_msgs/TransformStamped
std_msgs/Header header #头信息
uint32 seq #序列号
time stamp #时间戳
string frame_id #父级坐标名称
string child_frame_id #子级坐标名称
geometry_msgs/Transform transform #坐标信息
geometry_msgs/Vector3 translation #偏移量
float64 x
float64 y
float64 z
geometry_msgs/Quaternion rotation #四元数
float64 x
float64 y
float64 z
float64 w
1.2 geometry_msgs/PointStamped–>用于传输某个坐标系内坐标点的信息
命令行键入:rosmsg info geometry_msgs/PointStamped
std_msgs/Header header #头信息
uint32 seq #序号
time stamp #时间戳
string frame_id #所属坐标系的名称
geometry_msgs/Point point #固定坐标点坐标
float64 x
float64 y
float64 z
注:创建坐标变换功能包要添加的依赖roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs
2 . 静态坐标信息
所谓静态坐标变换,是指两个坐标系之间的相对位置是固定的。
要想实现静态坐标变换,可以通过话题通信发布订阅消息,发布方发布坐标系相对关系,订阅方订阅发布的坐标系相对关系,再传入坐标点信息(可以是固定的一点),借助TF实现坐标变换.
- 发布方核心:
创建静态发布对象–>tf2_ros::StaticTransformBroadcaster pub;
创建坐标系信息–>
geometry_msgs::TransformStamped ts;
(头信息、子级坐标系、偏移量、四元数)
发布消息–> pub.sendTransform(ts); - 订阅方核心:
创建订阅对象–>
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
创建一个坐标点(相对于子级)–>geometry_msgs::PointStamped ps;
坐标转换,需要新建一个坐标点(父级)接收转换结果–>
geometry_msgs::PointStamped ps_out;
ps_out = buffer.transform(ps_out,"父级坐标系名称");
- 注意:在进行坐标变换时,使用try语句或者在坐标变换前实行休眠,可以避免由于缓存接收延迟而导致坐标变换失败
3. 动态坐标变换
所谓动态坐标变换,是指两个坐标系之间的相对位置是变化的。
实现和静态坐标变换差不多,订阅方是一样的,发布方在实现的时候是要先订阅一个动态的对象(比如:小乌龟的位姿),然后回调函数处理订阅到的数据,处理过程和静态坐标变换发布方实现一样
4. 多坐标变换
多个坐标系之间的相对位置关系变换。例如:现有坐标系统,父级坐标系统 world,下有两子级系统 son1,son2,son1 相对于 world,以及 son2 相对于 world 的关系是已知的,求 son1原点在 son2中的坐标,又已知在 son1中一点的坐标,要求求出该点在 son2 中的坐标
实现:首先,需要发布 son1 相对于 world,以及 son2 相对于 world 的坐标消息,然后,需要订阅坐标发布消息,并取出订阅的消息,借助于 tf2 实现 son1 和 son2 的转换,最后实现坐标点的转换
关于发布方,用launch文件封装,且使用静态坐标发布变换
<launch>
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
</launch>
订阅方实现核心:
解析son1中的点相对于son2的坐标(坐标变换)
geometry_msgs::TransformStamped tfs =buffer.lookupTransform("son2","son1",ros::Time(0));
实现坐标点的转换
geometry_msgs::PointStamped ps; //创建一个坐标点(相对于子级son1)
geometry_msgs::PointStamped psAtSon2; //新建一个坐标点(相对父级son2)接收转换结果
psAtSon2 = buffer.transform(ps,"son2");
5 坐标关系查看
可以通过tf2_tools工具查看,安装指令:
sudo apt install ros-noetic-tf2-tools
启动坐标系发布程序之后,运行如下命令:rosrun tf2_tools view_frames.py
当前目录会生成一个 frames.pdf 文件。
案例实现: 乌龟例程中的TF变换
需求描述:运行程序,产生两只乌龟 turtle1 和 turtle2,键盘控制 turtle1 运动,turtle2 可以实现跟随运动
流程:
(1)启动乌龟显示节点
(2)编写服务客户端,生成一只新乌龟(test01_new_turtle)
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
//执行初始化
ros::init(argc,argv,"create_turtle2");
//创建节点
ros::NodeHandle nh;
//创建服务客户端
ros::ServiceClient client = nh.serviceClient<turtlesim::Spawn>("/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 = 1.57;
client.waitForExistence();
bool flag = client.call(spawn);
//处理响应
if (flag)
{
ROS_INFO("乌龟生成成功,新乌龟叫:%s",spawn.response.name.c_str());
} else
{
ROS_INFO("请求失败!!!");
}
return 0;
}
(3)编写发布方,发布两只乌龟相对于世界坐标系的坐标信息(test02_pub_turtle)
- 订阅乌龟的位姿信息,然后再转换成坐标信息,两只乌龟的实现逻辑相同,只是话题名称、各自的坐标不同,可通过参数传入:
该节点需要启动两次;
每次启动时都需要传入乌龟节点名称(第一次是 turtle1 第二次是 turtle2)
/*
该文件实现:需要订阅 turtle1 和 turtle2 的 pose,然后发布相对 world 的坐标系信息
实现流程:
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 pub;
// 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;
//坐标系四元数----欧拉角转换四元数
//乌龟位姿信息没有四元数,但有偏航角度,乌龟位姿的欧拉角:0 0 theta
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.发布消息
pub.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("请传入一个参数");
return 1;
} else {
turtle_name = argv[1];
}
// 4.创建 ros 句柄
ros::NodeHandle nh;
// 5.创建订阅对象--订阅乌龟的pose信息(动态传入乌龟名称)
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>(turtle_name + "/pose",100,doPose);
// 6.回调函数处理订阅的 pose 信息
// 7.spin
ros::spin();
return 0;
}
(4)编写订阅方,订阅两只乌龟信息并生成新的坐标相对关系生成速度信息
(test03_control_turtle)
#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"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
订阅 turtle1 和 turtle2 的发布信息,查找并转换时间最近的 TF 信息
将 turtle1 转换成相对 turtle2 的坐标,在计算线速度和角速度并发布
流程:
1.包含头文件
2.编码初始化、NodeHandle;
3.创建订阅对象
4.编写解析逻辑
5.spinOnce()
*/
int main(int argc, char *argv[])
{
// 2.编码初始化、NodeHandle;
setlocale(LC_ALL,"");
ros::init(argc,argv,"tfs_pub");
ros::NodeHandle nh;
// 3.创建订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
//A. 创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
//4.编写解析逻辑
ros::Rate rate(1);
// ros::Duration(2).sleep();
while (ros::ok())
{
//核心
try
{
//计算turtle1相对于turtle2的关系
geometry_msgs::TransformStamped son1ToSon2 = buffer.lookupTransform("turtle2","turtle1",ros::Time(0));
// ROS_INFO("turtle1相对于turtle2的信息:父级:%s,子级:%s 偏移量(%.2f,%.2f,%.2f)",
// son1ToSon2.header.frame_id.c_str(), //turtle2
// son1ToSon2.child_frame_id.c_str(), // turtle1
// son1ToSon2.transform.translation.x,
// son1ToSon2.transform.translation.y,
// son1ToSon2.transform.translation.z);
//B. 根据相对关系计算并组织速度消息
geometry_msgs::Twist twist;
/*
组织速度,只需要设置线速度的 x 与角速度的 z
x = 系数 * 开方(y^2 + x^2)
z = 系数 * 反正切(对边,邻边)
*/
twist.linear.x = 0.5 * sqrt(pow(son1ToSon2.transform.translation.x,2) + pow(son1ToSon2.transform.translation.y,2));
twist.angular.z = 1.0 * atan2(son1ToSon2.transform.translation.y,son1ToSon2.transform.translation.x);
//C. 发布
pub.publish(twist);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
// 5.spinOnce()
ros::spinOnce();
}
return 0;
}
(5)实现乌龟跟随运动
编写一个launch文件(test.launch),让所有节点运动起来
<launch>
<!--1.启动乌龟GUI-->
<node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
<node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />
<!--2.生成新的乌龟节点-->
<node pkg="tf04_test" type="test01_new_turtle" name="turtle2" output="screen" />
<!--3.需要启动两个乌龟相对于世界的坐标关系发布-->
<!--
基本实现思路:
1.节点编写一个
2.这个节点需要启动两次
3.节点启动时动态传参:第一次传递turtle1 第二次启动传递turtle2
-->
<node pkg="tf04_test" type="test02_pub_turtle" name="pub1" args="turtle1" output="screen" />
<node pkg="tf04_test" type="test02_pub_turtle" name="pub2" args="turtle2" output="screen" />
<!--4.订阅turtle1 与 turtle2 相对于世界坐标系的坐标消息,并转换成turtle1相对于turtle2的坐标关系
再生成速度关系
-->
<node pkg="tf04_test" type="test03_control_turtle2" name="control" output="screen" />
</launch>