ROS TF静态坐标变换实现

ROS TF静态坐标变换实现

法一：编码实现

①发布方代码实现：创建功能包并添加依赖

catkin_create_pkg tf_static roscpp rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs

②新建python文件，为文件添加可执行权限，修改CMakelists文件

③编写发布方代码，编译执行

#! /usr/bin/env python
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations #欧拉角获取四元数
"""
发布方实现：用于发布两个坐标系的新相对关系（小车底盘——baselink和雷达——laser）
流程：1、导包
    2、初始化ROS节点
    3、创建发布对象
    4、组织被发布的数据
    5、循环发布数据
    6、spin（）
"""
if __name__=="__main__":
    rospy.init_node("tf_static_pub")
    #创建发布对象
    pub = tf2_ros.StaticTransformBroadcaster()
    #组织被发布的数据
    ts = TransformStamped()
    #header
    ts.header.stamp=rospy.Time.now()#设置时间戳
    ts.header.frame_id = "baselink"#父级坐标系
    #child fram
    ts.child_frame_id = "laser"

    #相对关系（偏移和四元数）
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    #四元数：一般先设置欧拉角，再转换成四元数
    qtn = tf.transformations.quaternion_from_euler(0,0,0)#是一个列表对象，可以下标索引获取对应的值
    #四元数获取
    ts.transform.rotation.x =qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    #发布数据
    pub.sendTransform(ts)
    rospy.spin()

④查看发布方发布的数据

  rostopic list，可以看到有话题名为tf_static的话题

  rostopic echo tf_static可以看到发布的具体信息

  也可以借助可视化工具rviz直观看到两个坐标系：终端中输入rviz打开可视化窗口，左上角Fixed Frame选择baselink，左下角add，添加TF即可看到）

④订阅方代码编写

  新建python文件tf_static_sub.py，为文件添加可执行权限，修改CMakelists文件

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现：
订阅坐标变换消息，传入被转换的坐标点，调用转换算法实现坐标转换
1、导包（用到什么包导入什么包）
2、初始化ros节点
3、创建订阅对象
4、组织被转换的坐标点
5、转换逻辑编写（调用tf封装算法）
6、输出转换结果B
7、spin（）/spinonce（）
"""
if __name__=="__main__":
    #初始化ros节点
    rospy.init_node("static_tf_pub")
    #创建订阅对象
    buffer = tf2_ros.Buffer() #创建缓存对象
    sub = tf2_ros.TransformListener(buffer)#将缓存对象传入
    #组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()#可以用rosmsg info geometry_msgs.PointStamped 查看消息结构
    ps.header.stamp = rospy.Time.now()
    ps.header.frame_id = "laser"
    ps.point.x = 2.0
    ps.point.y = 3.0
    ps.point.z = 5.0
    #转换逻辑实现，由于雷达是不间断获取信息的，这里用循环发布数据来模拟
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #转换：buffer提供了对应的转换函数
            ps_out = buffer.transform(ps,"baselink")
            rospy.loginfo("转换后的坐标结果:(%.2f,%.2f,%.2f)当前参考的坐标系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id) 
        except:
            rospy.logwarn("错误提示：%s",Exception)
        rate.sleep()

法二：ros封装命令实现静态坐标转换

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 x轴偏转角 y轴偏转角 z轴偏转角 父坐标系名 子坐标系名

  可以实现像编码一样的效果，同样可以通过rostopic查看话题名和消息格式具体信息。也可以通过rviz可视化看到两坐标系的相对位置