单坐标静态变换
需求描述相对关系广播方节点订阅方节点广播的封装 单坐标动态变换
需求描述发布方节点启动订阅方节点 多坐标变换
需求描述发布方节点订阅方 查看节点位置关系
单坐标静态变换 需求描述创建项目功能包依赖于:
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),请问，该障碍物相对于主体的坐标是多少？
相对关系 广播方节点
##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; //传输的数据，是两个位置相对位置数据类型
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.包含头文件
##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;// 相对于原点位置数据类型和相对坐标位置不同
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
// 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("/turtle1/pose", 1000, doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
//
// 6.spin
ros::spin();
return 0;
}
多坐标变换
需求描述
现有坐标系统，父级坐标系统 world,下有两子级系统 son1，son2，son1 相对于 world，以及 son2 相对于 world 的关系是已知的，求 son1原点在 son2中的坐标，又已知在 son1中一点的坐标，要求求出该点在 son2 中的坐标
发布方节点订阅方
//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
生成的关系文档生成在运行命令的相对目录下
