题外话：
首先给大家道个歉，因为之前迷上了无人驾驶apollo仿真，花了一段时间研究。后来发现没有资源，没有设备，是真的难学，所以放弃了apollo。最近又到了开题答辩时间，花了老长时间和师兄互相折磨，可能还要耽误一段时间才能恢复更新。而且最近发现前面更新的感觉还是有些不够仔细，请放心，最后更完了会修改前面。马上要开嵌入式模块以及刷题模块，做一个欢快的学习仔。
感谢：
http://www.autolabor.com.cn/book/ROSTutorials/di-5-zhang-ji-qi-ren-dao-hang/51-tfzuo-biao-bian-huan.html
这篇博客大佬指出了tf还有一些可视化的命令
(51条消息) ROS中的常用组件（二）——TF坐标变换_我不是“耀”神的博客-CSDN博客_echo tf
在ROS中有一些小组件库，这些组件是关于实际机器人开发头文件，目前常用的是
TF坐标变换
Rosbag
rqt 工具箱5.1 TF坐标变换
如果你研究机器人的运动情况，那么TF坐标变换就一定要学习，因为这就是机器人运动相关的组件。就像某本书上说过：机器人的精密程度在一定情况下取决于传感器的精确程度，机器人运动就借助这些传感器进行的，传感器后面会提供一些使用情况，这里就不细说了。
我们现在已知ROS小车的某些传感器就能够提供这些信号，这些信号在运动学上表现得是欧拉角或者四元素。现在我们需要一个舞台来体现这种变换。
首先介绍的是头文件
geometry_msgs/TransformStamped 这个头文件实现的是坐标系和坐标系之间的关系，
tf2_ros/static_transform_broadcaster 用来广播上面坐标系之间的关系
tf2/LinearMath/Quaternion则是四元素和欧拉角之间的转换头文件
tf2_ros/transform_listener用来接受上面广播的tf关系5.1.1实现初始坐标位置
首先要新建功能包，然后在功能包里面输入
roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs
然后发送坐标之间的关系（只有发送出去才能在rviz和其他节点接收到）
/*
静态坐标变换发布方:
发布关于 laser 坐标系的位置信息
实现流程:
1.包含头文件
2.初始化 ROS 节点
3.创建静态坐标转换广播器
4.创建坐标系信息
5.广播器发布坐标系信息
6.spin()
*/
// 1.包含头文件
#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,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"static_brocast");
// 3.创建静态坐标转换广播器
tf2_ros::StaticTransformBroadcaster broadcaster;
// 4.创建坐标系信息
geometry_msgs::TransformStamped ts;
//----设置头信息
ts.header.seq = 100;
ts.header.stamp = ros::Time::now();
ts.header.frame_id = "base_link";//是坐标系的中心
//----设置子级坐标系（备注1）
ts.child_frame_id = "laser";
//----设置子级相对于父级的偏移量
ts.transform.translation.x = 0.2;
ts.transform.translation.y = 0.0;
ts.transform.translation.z = 0.5;
//----设置四元数:将 欧拉角数据转换成四元数
tf2::Quaternion qtn;
qtn.setRPY(0,0,0);//这个对象能够使欧拉角转换为四元素
ts.transform.rotation.x = qtn.getX();
ts.transform.rotation.y = qtn.getY();
ts.transform.rotation.z = qtn.getZ();
ts.transform.rotation.w = qtn.getW();//欧拉角000-转换成四元素0001
// 5.广播器发布坐标系信息
broadcaster.sendTransform(ts);
ros::spin();
return 0;
}备注1:这里的子坐标系其实就是参考中心坐标系的变换情况，上面的代码主要是设定一些基础的参数，小车中心坐标系在传感器（主坐标系）的某个位姿
可以运行roscore使用rostopic list以及rostopic echo /tf_static来查看
当然还可以通过图形化界面来查看
输入rviz图形化界面，设置fixed frame为著坐标系，点击add找到tf来查看
5.1.2实现接受信息
实现一个初始坐标的设定之后，其他的设备可能会需要接受这一设定信息（在雷达的世界坐标系到小车的坐标系上），接收方就按照坐标基础实现其坐标系之间的静态转换
发送方实现了（laser到base_link之间的变化情况），这里简单的介绍一下接收方的实现（实现某点在laser 上的坐标到base_link坐标）
#include "ros/ros.h"
#include "tf2_ros/transform_listener.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.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);
// 5.解析订阅信息中获取 在laser坐标系中的坐标信息在
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::Rate rate(10);
ros::Duration(2).sleep();//休眠两秒
ros::Rate r(10);//设置循环频率
while (ros::ok())
{
geometry_msgs::PointStamped ps_out;
ps_out=buffer.transform(ps," base_link ");
// r.sleep();
ROS_INFO("转换后x=%.2f,y=%.2f,z=%.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();
}
return 0;
}如果报以下错就是需要加延迟
5.1.3补充
在实际运动中不会简单是两个坐标系，往往是三个以上坐标系，这里就指出了在世界坐标系下有两个son坐标系，大致类型是一样的就不细说了
/*
需求:
现有坐标系统，父级坐标系统 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);
// 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();
ps.point.x = 1.0;
ps.point.y = 2.0;
ps.point.z = 3.0;
geometry_msgs::PointStamped psAtSon2;
psAtSon2 = buffer.transform(ps,"son2");
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;
}5.1.4动态坐标变化
在上面只是简单的使用了静态坐标的方法，这里将要控制小乌龟实现动态坐标变换
一个问题是如何获取动物体，哪里有动物？：
前面说过小乌龟，可以用键盘控制小乌龟，然后获取运动信息
问题：怎么获取小乌龟的位姿信息
//在视频里说过：启动一个小品乌龟使用的是
//和虚拟键盘…，通过使用rostopic能够看到或获取小乌龟位姿是pose
//因此查看这个pose，也就是说小乌龟生成的时候就已经有一个世界坐标系了，小乌龟运动就是在这个坐标系下的运动
//使用rostopic info /turtle1/pose可以看到格式
是这种类型，所以rosmsg info …/…即
简单说一下：窗口的左下角是坐标原点，而这个pose则是小乌龟在原点上的坐标信息
新建功能包
tf2、tf2_ros、tf2_geometry_msgs、roscpp rospy std_msgs geometry_msgs、turtlesim
发送方：小乌龟坐标系在世界坐标系的坐标是pose，这个pose消息是时刻变化的，所以二者之间的关系不仅需要广播出去，还需要注意这个值是变化的
// 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);//偏航角是pose的偏航角
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.创建订阅对象
ros::Subscriber sub = nh.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose);
// 5.回调函数处理订阅到的数据(实现TF广播)
//
// 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 必须包含该头文件
int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
// 2.初始化 ROS 节点
ros::init(argc,argv,"dynamic_tf_sub");
ros::NodeHandle nh;
// 3.创建 TF 订阅节点
tf2_ros::Buffer buffer;
tf2_ros::TransformListener listener(buffer);
ros::Rate r(1);
while (ros::ok())
{
// 4. 子级坐标系生成一个坐标点
geometry_msgs::PointStamped point_laser;
point_laser.header.frame_id = "turtle1";
point_laser.header.stamp = ros::Time();//这个地方有时间匹配问题
point_laser.point.x = 1;
point_laser.point.y = 1;
point_laser.point.z = 0;
// 5. 父级坐标系是转换得到的，因为我们无法得到窗口的原点
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"world");
ROS_INFO("坐标点相对于 world 的坐标为:(%.2f,%.2f,%.2f)",point_base.point.x,point_base.point.y,point_base.point.z);
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常:%s",e.what());
}
r.sleep();
ros::spinOnce();
}
return 0;
}在已经打开小乌龟的窗口下执行上面两个文件，并用键盘控制
这样接受方的转换坐标系出现了变化（一开始是随机生成的）
要注意：buffer在进入时是有延迟的，在转换的时候时间如果时间匹配不上，就会出问题
处理实时有延迟，
当然，还可以打开rviz，配置世界坐标系加上选择TF即可
有个小问题：因为窗口是按照左下角是世界坐标系，也就意味着在实际运动的时候固定在1/4区域移动
5586
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
