P186静态坐标变换 python 发布方（发布两个坐标系相对关系）

最新推荐文章于 2024-10-03 09:02:12 发布

叶子202422

最新推荐文章于 2024-10-03 09:02:12 发布

阅读量670

点赞数 15

分类专栏： ROS1学习记录文章标签： python 开发语言

本文链接：https://blog.csdn.net/qq_46781387/article/details/140333796

版权

ROS1学习记录专栏收录该内容

8 篇文章 0 订阅

订阅专栏

1.代码

#！ /usr/bin/env python

# 1.导包
import rospy
import tf.transformations
import tf2_ros
import tf
from geometry_msgs.msg import TransformStamped

""" 
    发布方：发布两个坐标系的相对关系（车辆底盘--base_link 和 雷达--laser
    流程：
        1.导包
        2.初始化节点
        3.创建发布者对象
        4.组织被发布的数据
        5.发布数据
        6.spin()
        #若不添加spin()，节点发布完，直接结束了。它的作用是启动一个 ROS 节点的事件处理循环，等待相应回调函数的触发
 """




if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_pub_p")
    # 3.创建发布对象  静态坐标广播器
    pub = tf2_ros.StaticTransformBroadcaster()#需要导包tf2_ros
    # 4.创建并组织被发布的消息
    #消息是geometry_msgs/TransformStamped，需要导包geometry_msgs.msg
    ts = TransformStamped()
    #消息格式见5.1.1
    # --- 头信息
    # header
    #tf.header.seq = 101#seq序列号可不用设置
    ts.header.stamp = rospy.Time.now()
    ts.header.frame_id = "base_link"#父坐标系
    
    # --- 子坐标系
    #child frame
    ts.child_frame_id = "laser"
    # --- 坐标系相对信息
    # ------ 偏移量(子坐标系相对父坐标系，在xyz轴上的偏移量)
    ts.transform.translation.x = 0.2
    ts.transform.translation.y = 0.0
    ts.transform.translation.z = 0.5
    # ------ 四元数
    #子坐标系相对父坐标线上，角度的偏移；翻滚x，俯仰y，偏航z
    #一般不直接设置四元数，先设置欧拉角（本次雷达相对小车均为0），再转换为四元数（此时xyz均为0，w=1）
    #需要导包tf
    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]

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