创建项目功能包依赖于:
tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs
单坐标静态变换
需求描述
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
相对关系
广播方节点
/*
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建静态坐标转换广播器
4.创建坐标系信息
5.广播器发布坐标系信息
6.spin()
*/
##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[])
{
setlocale(LC_ALL, "");
ros::init(argc, argv, "static_pub");
ros::NodeHandle nh;
tf2_ros::StaticTransformBroadcaster pub; //类似ros::Publisher
geometry_msgs::TransformStamped tfs; //传输的数据，是两个位置相对位置数据类型
/*
表示：child_frame_id在frame_id的那个地方？包括距离和方向
std_msgs/Header header ##头信息
uint32 seq ##|-- 序列号
time stamp ##|-- 时间戳
string frame_id ##|-- 坐标 ID
string child_frame_id ##子坐标系的 id
geometry_msgs/Transform transform ##坐标信息
geometry_msgs/Vector3 translation ##偏移量
float64 x ##|-- X 方向的偏移量
float64 y ##|-- Y 方向的偏移量
float64 z ##|-- Z 方向上的偏移量
geometry_msgs/Quaternion rotation ##四元数
float64 x
float64 y
float64 z
float64 w
注意：因为角度表示一般是我们熟知的欧拉角，但是需要四元数传输要调用tf2::Quaternion对象中的setRPY函数转化获取
*/
tfs.header.seq = 100;
tfs.header.stamp = ros::Time::now();
tfs.header.frame_id = "base_link";
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, 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;
}
订阅方节点
/*
订阅坐标系信息，生成一个相对于 子级坐标系的坐标点数据，转换成父级坐标系中的坐标点
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 TF 订阅节点
4.生成一个坐标点(相对于子级坐标系)
5.转换坐标点(相对于父级坐标系)
6.spin()
*/
//1.包含头文件
##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 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL, "");
// 2.初始化 ROS 节点
ros::init(argc, argv, "tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
//这里实例化并接受pub广播的信息后的buffer中存储着base_link和laser的相对位置
//可以用于坐标位置和相对位置生成指定的坐标位置
ros::Rate r(1);
while (ros::ok())
{
// 4.生成一个坐标点(相对于子级坐标系)
geometry_msgs::PointStamped point_laser;// 相对于原点位置数据类型和相对坐标位置不同
/*
std_msgs/Header header #头
uint32 seq #|-- 序号
time stamp #|-- 时间戳
string frame_id #|-- 所属坐标系的 id
geometry_msgs/Point point #点坐标
float64 x #|-- x y z 坐标
float64 y
float64 z
*/
point_laser.header.frame_id = "laser"; // 坐标点名称
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 2; //物体相对于laser的坐标值
point_laser.point.y = 3;
point_laser.point.z = 5;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base; //存储目标位置（相对base_link）
//buffer中base_link和laser两个相对位置和point_laser（障碍物相对于laser位置即坐标位置）生成障碍物相对于base_link的位置并赋值给point_base
point_base = buffer.transform(point_laser, "base_link");//包含头文件##include "tf2_geometry_msgs/tf2_geometry_msgs.h"
ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:%s",point_base.point.x, point_base.point.y, point_base.point.z, point_base.header.frame_id.c_str());
}
catch (const std::exception &e)
{
ROS_INFO("程序异常.....");
}
r.sleep();
ros::spinOnce();
}
return 0;
}
广播的封装
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 /base_link /laser
这一句就相当于广播方节点发布
静态写入launch文件
单坐标动态变换
需求描述
启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘控制乌龟运动，将两个坐标系的相对位置动态发布。
发布方节点启动
rosrun turtlesim turtlesim_node
订阅方节点
/*
动态的坐标系相对姿态发布(一个坐标系相对于另一个坐标系的相对姿态是不断变动的)
需求: 启动 turtlesim_node,该节点中窗体有一个世界坐标系(左下角为坐标系原点)，乌龟是另一个坐标系，键盘
控制乌龟运动，将两个坐标系的相对位置动态发布
实现分析:
1.乌龟本身不但可以看作坐标系，也是世界坐标系中的一个坐标点
2.订阅 turtle1/pose,可以获取乌龟在世界坐标系的 x坐标、y坐标、偏移量以及线速度和角速度
3.将 pose 信息转换成 坐标系相对信息并发布
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建 ROS 句柄
4.创建订阅对象
5.回调函数处理订阅到的数据(实现TF广播)
5-1.创建 TF 广播器
5-2.创建 广播的数据(通过 pose 设置)
5-3.广播器发布数据
6.spin
*/
// 1.包含头文件
#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"
void doPose(const turtlesim::Pose::ConstPtr &pose)
{
// 5-1.创建 TF 广播器
static tf2_ros::TransformBroadcaster broadcaster;
// 5-2.创建 广播的数据(通过 pose 设置)
geometry_msgs::TransformStamped tfs;
// |----头设置
tfs.header.frame_id = "world";
tfs.header.stamp = ros::Time::now();
// |----坐标系 ID
tfs.child_frame_id = "turtle1";
// |----坐标系相对信息设置
tfs.transform.translation.x = pose->x;
tfs.transform.translation.y = pose->y;
tfs.transform.translation.z = 0.0; // 二维实现，pose 中没有z，z 是 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();
// 5-3.广播器发布数据
broadcaster.sendTransform(tfs);
}
int main(int argc, char *argv[])
{
setlocale(LC_ALL, "");
// 2.初始化 ROS 节点
ros::init(argc, argv, "dynamic_tf_pub");
// 3.创建 ROS 句柄
ros::NodeHandle nh;
// 4.创建订阅对象,第一个乌龟位置的默认话题是/turtle1/pose
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose", 1000, doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
//
// 6.spin
ros::spin();
return 0;
}
多坐标变换
需求描述
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
发布方节点
<launch>
<!--tf2_ros是相对坐标位置关系，args参数分别是三个坐标位置，三个方向位置 主节点名 子节点名-->
<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="son2" args="0.5 0 0 0 0 0 /world /son2" output="screen" />
</launch>
订阅方
/*
需求:
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，
son1 相对于 world，以及 son2 相对于 world 的关系是已知的，
求 son1 与 son2中的坐标关系，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
实现流程:
1.包含头文件
2.初始化 ros 节点
3.创建 ros 句柄
4.创建 TF 订阅对象
5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
解析 son1 中的点相对于 son2 的坐标
6.spin
*/
//1.包含头文件
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "geometry_msgs/TransformStamped.h"
#include "geometry_msgs/PointStamped.h"
int main(int argc, char *argv[])
{ setlocale(LC_ALL,"");
// 2.初始化 ros 节点
ros::init(argc,argv,"sub_frames");
// 3.创建 ros 句柄
ros::NodeHandle nh;
// 4.创建 TF 订阅对象
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);//buffer相当于一个集和，在里面搜索坐标位置参数
// 5.解析订阅信息中获取 son1 坐标系原点在 son2 中的坐标
ros::Rate r(1);
while (ros::ok())
{
try
{
// 解析 son1 中的点相对于 son2 的坐标
geometry_msgs::TransformStamped tfs = buffer.lookupTransform("son2","son1",ros::Time(0));
ROS_INFO("Son1 相对于 Son2 的坐标关系:父坐标系ID=%s",tfs.header.frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:子坐标系ID=%s",tfs.child_frame_id.c_str());
ROS_INFO("Son1 相对于 Son2 的坐标关系:x=%.2f,y=%.2f,z=%.2f",
tfs.transform.translation.x,
tfs.transform.translation.y,
tfs.transform.translation.z
);
// 坐标点解析
geometry_msgs::PointStamped ps;
ps.header.frame_id = "son1";
ps.header.stamp = ros::Time::now();
// 当xyz都等于0时，调用buffer.transform相当于buffer.lookupTransform方法,不过返回类型相同，不过没角度
ps.point.x = 1.0;
ps.point.y = 2.0;
ps.point.z = 3.0;
geometry_msgs::PointStamped psAtSon2;
psAtSon2 = buffer.transform(ps,"son2");//buffer中实际上有很多参数坐标，只是在这些集合中搜索目标位置
ROS_INFO("在 Son2 中的坐标:x=%.2f,y=%.2f,z=%.2f",
psAtSon2.point.x,
psAtSon2.point.y,
psAtSon2.point.z
);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("异常信息:%s",e.what());
}
r.sleep();
// 6.spin
ros::spinOnce();
}
return 0;
}
查看节点位置关系
- 图形界面
rviz
- pdf文档
rosrun tf2_tools view_frames.py
注意安装：sudo apt-get install ros-$ROS_DISTRO-tf2-tools
生成的关系文档生成在运行命令的相对目录下
5469
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
