目录
- 引子
- ros中的右手坐标系
- 补充：
- 欧拉角及四元数理解
- 旋转
- 平移操作
- 复合操作
- 运行坐标变换的例子
- 坐标转换
- 静态坐标变换-发布坐标系信息
- 创建功能包
- 静态坐标变换-订阅坐标系信息
- 添加cpp订阅者主文件
- 修改cmakelist
- 文件编译报错的解决方案
- 运行程序进行测试
引子
机器人系统上，有多个传感器，如激光雷达、摄像头等，有的传感器是可以感知机器人周边的物体方位(或者称之为:坐标，横向、纵向、高度的距离信息)的，以协助机器人定位障碍物，可以直接将物体相对该传感器的方位信息，等价于物体相对于机器人系统或机器人其它组件的方位信息吗？显示是不行的，这中间需要一个转换过程。
ros中的右手坐标系
tf:TransForm Frame,坐标变换
坐标系:ROS 中是通过坐标系统开标定物体的，确切的将是通过右手坐标系来标定的。
补充：
欧拉角及四元数理解
欧拉角：
对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。
在空间中会有n+1个坐标系，其中只有一个坐标系起到标定作用，也就是说“其他n个坐标系全都是基于该坐标系找到自己在空间中的位置的”。只有大家都知道了自己在空间中的具体位置，坐标转换才可以顺利进行下去。
在描述机器人运动时，我们常常提及“位姿”，其实位姿是一个合成词，我们可以将其拆解为“位置+姿态”。位置就是指“机器人某个运动关节/测量传感器在世界坐标系中的具体位置，姿态就是”基于该点的坐标系相较于世界坐标系所进行的旋转“。
四元数就比较复杂了，可以参考：
解析四元数
四元数的应用
四元数。就是通过四元数连乘的形式表达机器人末端相对于基坐标系的关系，或者先有关系，再通过求逆的运算求解每个关节应页的位置关系。四元数只能表示旋转，不能表示平移。
旋转
坐标变换，当没有平移，只有旋转操作时，可以理解为一个向量在原坐标系中的投影，又在另一个坐标系中的投影：
其中：
是坐标变换的旋转矩阵，将向量P在原B坐标系变换至A坐标系中。其组成是【X Y Z】即单位化的B坐标系的轴在A坐标系中的投影。
平移操作
复合操作
运行坐标变换的例子
roslaunch turtle_tf2 turtle_tf2_demo_cpp.launch
启动乌龟跟随例子窗口。
坐标转换
ros中封装了现成的坐标变换模块。
在坐标转换实现中常用的 msg:
（1）geometry_msgs/TransformStamped：
用于传输坐标系相对位置信息
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
translation指child_frame_id 相对于frame_id 的变化
（2）geometry_msgs/PointStamped
用于传输某个坐标系内坐标点的信息
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
x,y,z是坐标点的具体坐标值。
具体结构都可以用rosmsg info查看
静态坐标变换-发布坐标系信息
静态坐标变换
所谓静态坐标变换，是指两个坐标系之间的相对位置是固定的。
坐标系相对关系，可以通过发布方发布
订阅方，订阅到发布的坐标系相对关系，再传入坐标点信息(可以写死)，然后借助于 tf 实现坐标变换，并将结果输出。
场景：
现有一机器人模型，核心构成包含主体与雷达，各对应一坐标系，坐标系的原点分别位于主体与雷达的物理中心，已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物，在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问，该障碍物相对于主体的坐标是多少？
创建功能包
catkin_create_pkg tfdemo_publisher std_msgs rospy roscpp tf2 tf2_ros tf2_geometry_msgs geometry_msgs
除了基本依赖以外，新增了若干坐标系相关的依赖。
编写c++文件
// 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[])
{
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";//相对坐标系关系中被参考的那一个
//----设置子级坐标系
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;
//使用该对象设置欧拉角，然后调用函数将欧拉角转换为四元数，这里欧拉角刚好都是0，这里的单位都是弧度
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();
// 5.广播器发布坐标系信息
broadcaster.sendTransform(ts);
//静态的发布一次即可，当ROS节点调用ros::spin()时，它会一直等待直到有消息到达该节点。当节点接收到消息时，
//它将调用与该消息相关联的回调函数进行处理。这使得ROS节点能够响应其他节点的请求，同时保持事件循环处于活动状态。
//也就是维持一个事件循环
ros::spin();
return 0;
}
还有cmakelist文件：
add_executable(${PROJECT_NAME}_node src/demo01.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
编译并运行工程：
source /root/work/ws/devel/setup.bash
roscore
rosrun tfdemo_publisher tfdemo_publisher_node
如果roscore提示已启动，可使用killall -9 rosmaster杀掉重启。
查询当前话题：
rostopic list
得到：
/rosout
/rosout_agg
/tf_static
查看：
rostopic echo /tf_static
得到：
transforms:
-
header:
seq: 100
stamp:
secs: 1723624279
nsecs: 917443332
frame_id: "base_link"
child_frame_id: "laser"
transform:
translation:
x: 0.2
y: 0.0
z: 0.5
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
或者在rviz里也可以查看。
静态坐标变换-订阅坐标系信息
订阅demo01内的坐标系相对关系，传入坐标点，再进行转换。
添加cpp订阅者主文件
为了简便，该订阅者cpp文件与发布者在同一个包的同一个src下，
//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 订阅节点,listener订阅数据，并缓存至Buffer当中
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 = "laser";
point_laser.header.stamp = ros::Time::now();
point_laser.point.x = 1;
point_laser.point.y = 2;
point_laser.point.z = 7.3;
// 5.转换坐标点(相对于父级坐标系)
//新建一个坐标点，用于接收转换结果
//--------------使用 try 语句或休眠，否则可能由于缓存接收延迟而导致坐标转换失败------------------------
try
{
geometry_msgs::PointStamped point_base;
point_base = buffer.transform(point_laser,"base_link");
ROS_INFO("转换后的数据:(%.2f,%.2f,%.2f),参考的坐标系是:",point_base.point.x,point_base.point.y,point_base.point.z,point_base.header.frame_id.c_str());
}
catch(const std::exception& e)
{
// std::cerr << e.what() << '\n';
ROS_INFO("程序异常.....");
}
r.sleep();
ros::spinOnce();
}
return 0;
}
修改cmakelist
新增节点2即可：
add_executable(${PROJECT_NAME}_node src/demo01.cpp)
add_executable(${PROJECT_NAME}_node2 src/demo02.cpp)
target_link_libraries(${PROJECT_NAME}_node
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_node2
${catkin_LIBRARIES}
)
文件编译报错的解决方案
如果碰到：./boost/thread/pthread/thread_data.hpp:60:5: error: missing binary operator before token “(“报错
即可编译通过。
运行程序进行测试
source /root/work/ws/devel/setup.bash
roscore
rosrun tfdemo_publisher tfdemo_publisher_node
rosrun tfdemo_publisher tfdemo_publisher_node2
