目录
- 引子
- 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 yfloat64 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: 100stamp: secs: 1723624279nsecs: 917443332frame_id: "base_link"child_frame_id: "laser"transform: translation: x: 0.2y: 0.0z: 0.5rotation: x: 0.0y: 0.0z: 0.0w: 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