translation:平移量 rotation:翻转量
一、静态坐标变换
1.概述
两个坐标系之间的相对位置是固定的,知道主体的坐标和雷达的坐标,通过雷达感知可以推算出主体距离障碍物的距离是多少
2.发布方发布
#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[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "static_pub");
ros::NodeHandle nh;
//创建发布对象
tf2_ros::StaticTransformBroadcaster pub;
geometry_msgs::TransformStamped tfs;
//stamp指时间戳,now可以获取当前时刻
tfs.header.stamp = ros::Time::now();
//frame指的是相对坐标系中被参考的那一个
tfs.header.frame_id = "base";
//child_frame表示雷达的坐标系
tfs.child_frame_id="laser";
//偏移量
tfs.transform.translation.x=0.2;
tfs.transform.translation.y=0.0;
tfs.transform.translation.z=0.5;
//四元数根据欧拉角来转换
tf2::Quaternion qtn; //创建四元数对象
//向该对象设置欧拉角
qtn.setRPY(0,0,0); //单位是弧度!
tfs.transform.rotation.x=qtn.getX();
tfs.transform.rotation.y=qtn.getY();
tfs.transform.rotation.z=qtn.getZ();
tfs.transform.rotation.w=qtn.getW();
//发布数据
pub.sendTransform(tfs);
ros::spin();
return 0;
}
3.订阅方订阅(tf算法实现坐标变换)
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h" //创建订阅对象(监听)
#include "tf2_ros/buffer.h" //将订阅到的数据存到buffer中
#include "geometry_msgs/PointStamped.h" //组织一个坐标点数据
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //tf自己封装的消息
int main(int argc, char *argv[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "static_sub");
ros::NodeHandle nh; //必须创建,内部函数需要用到
//创建订阅对象(需要对象+缓存)
//创建buffer缓存
tf2_ros::Buffer buffer;
//创建监听对象
tf2_ros::TransformListener listener(buffer);
//创建坐标点对象
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();
//转换算法,需要调用TF内置实现
ros::Rate rate(10); //发布频率10Hz
while(ros::ok())
{
//将ps转换为base(pub中)的坐标点
geometry_msgs::PointStamped ps_out; //输出的坐标
/*buffer.transform()
第一个参数为被转化的坐标,
第二个参数为视为参考点的名称
返回值是转化后的坐标
*/
ps_out=buffer.transform(ps,"base");
ROS_INFO("参考坐标系为:%s 转化后的坐标值:%.2f, %.2f, %.2f",
ps_out.header.frame_id.c_str(),
ps_out.point.x,
ps_out.point.y,
ps_out.point.z);
rate.sleep();
ros::spinOnce();
}
return 0;
}
//调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
4.可能会遇到的问题
(1)由于发布的坐标点不能马上被订阅方订阅到,所以在运行发布方后,可能会报错,因此我们需要再发布完坐标点后,让发布方休眠一段时间。
二、动态坐标变换
1.概述
获取动态的相对坐标
2.坐标发布方(同时需要订阅乌龟位姿信息)
#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
消息:/turtlesim/Pose
*/
void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
{
//获取位姿信息,转化为坐标系相对关系并发布
//创建TF发布对象
static tf2_ros::TransformBroadcaster pub; //加static是为了防止每次进入回调函数都要重新定义一次
//创建发布数据:从参数pose中进行转换
geometry_msgs::TransformStamped ts;
ts.header.frame_id="world"; //参考的坐标系(名字是固定的)
ts.header.stamp=ros::Time::now();
ts.child_frame_id="turtle1"; //相对的坐标系(名字是固定的)
//坐标系平移量的设置
ts.transform.translation.x = pose->x; //这些值都可以通过rosmsg info turtlesim/Pose来查看的
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();
//发布
pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "dynamic_pub");
ros::NodeHandle nh;
//参数一:话题名称 参数二:队列长度 参数三:回调函数
ros::Subscriber sub=nh.subscribe("turtle1/pose",100,PoseCallback);
ros::spin();
return 0;
}
3.坐标订阅方
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h" //创建订阅对象(监听)
#include "tf2_ros/buffer.h" //将订阅到的数据存到buffer中
#include "geometry_msgs/PointStamped.h" //组织一个坐标点数据
#include "tf2_geometry_msgs/tf2_geometry_msgs.h" //tf自己封装的消息
int main(int argc, char *argv[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "static_sub");
ros::NodeHandle nh; //必须创建,内部函数需要用到
//创建订阅对象(需要对象+缓存)
//创建buffer缓存
tf2_ros::Buffer buffer;
//创建监听对象
tf2_ros::TransformListener listener(buffer);
//创建坐标点对象
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=5.0;
//添加休眠:以便坐标点被订阅方订阅(这个不加会报错!)
ros::Duration(2).sleep();
//转换算法,需要调用TF内置实现
ros::Rate rate(10); //发布频率10Hz
while(ros::ok())
{
//将ps转换为base(pub中)的坐标点
geometry_msgs::PointStamped ps_out; //输出的坐标
/*buffer.transform()
第一个参数为被转化的坐标,
第二个参数为视为参考点的名称
返回值是转化后的坐标
*/
ps_out=buffer.transform(ps,"world");
ROS_INFO("参考坐标系为:%s 转化后的坐标值:%.2f, %.2f, %.2f",
ps_out.header.frame_id.c_str(),
ps_out.point.x,
ps_out.point.y,
ps_out.point.z);
rate.sleep();
ros::spinOnce();
}
return 0;
}
//调用时必须包含头文件 tf2_geometry_msgs/tf2_geometry_msgs.h
4.可能会遇到的问题
(1)时间戳不可以实时发布
三、多坐标变换
1.概述
父级坐标world下,有两个子级坐标,已知两个子级坐标相对于world的相对坐标,求一个子级相对于另一个子级的坐标
2.发布方
我们采用.launch文件的形式,直接发布对象
<launch>
<!--发布son1相对于world 以及 son2相对于world的坐标关系-->
<node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="5 0 0 0 0 0 /world /son1" output="screen"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="son2" args="3 0 0 0 0 0 /world /son2" output="screen"/>
</launch>
该情况下,我们发布的
son1坐标为(5,0,0),欧拉角为(0,0,0)
son2坐标为(3,0,0),欧拉角为(0,0,0)
使用时,直接启动laun文件即可
3.订阅方
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PoseStamped.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[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "turtles");
ros::NodeHandle nh;
//创建订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
//编写解析逻辑
ros::Rate rate(10);
//创建相对于son1的坐标点
geometry_msgs::PointStamped psson1;
psson1.header.stamp =ros::Time::now();
psson1.header.frame_id="son1";
psson1.point.x=1.0;
psson1.point.y=2.0;
psson1.point.z=3.0;
while(ros::ok())
{
try //可以避免因为发布订阅不同步造成的报错
{
//计算son1与son2的相对关系
/*buffer.lookupTransform()
参数1:根据该坐标系进行转化(父级)
参数2:原坐标系(子级)
参数3:何时的相对坐标
返回值:geometry_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);
//计算son1中的某个坐标点在son2中的坐标值
//创建相对于son2坐标点,然后将son1的转化为相对于son2的
geometry_msgs::PointStamped psson2 = buffer.transform(psson1,"son2");
ROS_INFO("son1坐标点在son2中的值: %.2f, %.2f, %.2f",
psson2.point.x, psson2.point.y, psson2.point.z);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}
四、坐标系关系查看
1.rviz图像化显示
可以得到如下界面
2.tf2_tools 功能包
(1)安装:
roslin@roslin-VirtualBox:~$ sudo apt install ros-noetic-tf2-tools
(2)启动完程序后生成pdf文件:
roslin@roslin-VirtualBox:~$ rosrun tf2_tools view_frames.py
(3)终端中打开pdf文件:
roslin@roslin-VirtualBox:~$ evince frames.pdf
得到如下界面:
五、TF坐标变换实操
1.概述:
实时将动态的相对坐标信息发送给观察者,观察者根据实时更新的坐标信息,匹配到相对应的速度信息,进而实现有效跟随。
2.生成一只新的乌龟:
#include "ros/ros.h"
#include "turtlesim/Spawn.h"
int main(int argc, char *argv[])
{
/* code */
ros::init(argc,argv,"newturtle");
ros::NodeHandle nh;
ros::service::waitForService("/spawn"); //等待发现/spawn服务
ros::ServiceClient add_turtle = nh.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
srv.request.x=2.0;
srv.request.y=2.0;
srv.request.name="turtle2";
//请求服务调用
ROS_INFO("Call service to spawn turtle[x:%0.6f, y:%0.6f, name:%s]",
srv.request.x, srv.request.y, srv.request.name);
add_turtle.call(srv);
//显示服务调用结果
ROS_INFO("Spawn turtle successfully [name:%s]",srv.response.name.c_str());
return 0;
}
3.发布方(发布两只乌龟的坐标信息)
#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
消息:/turtlesim/Pose
*/
//声明变量,接收传递的参数
std::string turtle_name;
void PoseCallback(const turtlesim::Pose::ConstPtr& pose)
{
//获取位姿信息,转化为坐标系相对关系并发布
//创建TF发布对象
static tf2_ros::TransformBroadcaster pub; //加static是为了防止每次进入回调函数都要重新定义一次
//创建发布数据:从参数pose中进行转换
geometry_msgs::TransformStamped ts;
ts.header.frame_id="world"; //参考的坐标系(名字是固定的)
ts.header.stamp=ros::Time::now();
//=====turtle1和turtle2是动态传入的====
ts.child_frame_id=turtle_name; //相对的坐标系(名字是固定的)
//坐标系平移量的设置
ts.transform.translation.x = pose->x; //这些值都可以通过rosmsg info turtlesim/Pose来查看的
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();
//发布
pub.sendTransform(ts);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc, argv, "dynamic_pub");
ros::NodeHandle nh;
/*
要想动态传入turtle1和turtle2,需要解析launch文件
*/
if(argc !=2 )
{
ROS_ERROR("请传入一个参数");
return 1;
}
else
{
turtle_name = argv[1];
}
//创建订阅对象
//====turtle1和turtle2是动态传入的====
//参数一:话题名称 参数二:队列长度 参数三:回调函数
//注意:动态传入话题时,pose前要加/ !!!
ros::Subscriber sub=nh.subscribe(turtle_name+"/pose",100,PoseCallback);
ros::spin();
return 0;
}
4.控制乌龟跟随
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_ros/buffer.h"
#include "geometry_msgs/PoseStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/Twist.h"
/*
1.换算turtle1相对于turtle2的关系 2.计算角速度和线速度并发布
*/
int main(int argc, char *argv[])
{
/* code */
setlocale(LC_ALL,"");
ros::init(argc, argv, "turtles");
ros::NodeHandle nh;
//创建订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener sub(buffer);
//创建发布对象
ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",100);
//编写解析逻辑
ros::Rate rate(10);
while(ros::ok())
{
try //可以避免因为发布订阅不同步造成的报错
{
//计算son1与son2的相对关系
/*buffer.lookupTransform()
参数1:根据该坐标系进行转化(父级)
参数2:原坐标系(子级)
参数3:何时的相对坐标
返回值:geometry_msgs::TransformStamped 子级相当于父级的相对坐标关系
*/
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(),
son1toson2.child_frame_id.c_str(),
son1toson2.transform.translation.x,
son1toson2.transform.translation.y,
son1toson2.transform.translation.z);
//根据相对坐标计算并组织速度消息
geometry_msgs::Twist twist;
/*
twist.linear.x = 系数*开二次方(x^2+y^2)
twist.angular.z = 系数*反正切(对边/邻边)
*/
twist.linear.x = 0.5 * sqrt(pow(son1toson2.transform.translation.x,2)+pow(son1toson2.transform.translation.y,2));
twist.angular.z = 4*atan2(son1toson2.transform.translation.y,son1toson2.transform.translation.x);
//发布
pub.publish(twist);
}
catch(const std::exception& e)
{
ROS_INFO("错误提示:%s",e.what());
}
rate.sleep();
ros::spinOnce();
}
return 0;
}