用Python实现ROS中tf坐标系广播与监听

2024-01-23 21:04:13

ROS入门：tf坐标系广播与监听的编程实现

我们用Python来实现tf坐标系广播与监听。

　　第一步，导入必要的库。

import rospy
import tf2_ros
import geometry_msgs.msg

　　第二步，创建一个ROS节点。

rospy.init_node('tf_broadcaster')

　　第三步，创建一个tf广播器。

br = tf2_ros.TransformBroadcaster()

　　第四步，创建一个tf监听器。

listener = tf2_ros.TransformListener()

　　第五步，创建一个定时器，每隔一段时间执行一次回调函数。

rate = rospy.Rate(10)

　　第六步，定义回调函数。

def callback(data):
    # 获取海龟的相对于world的位置
    turtle_pose = data.pose

    # 创建tf的广播器
    t = geometry_msgs.msg.TransformStamped()

    # 初始化tf
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "world"
    t.child_frame_id = "turtle1"
    t.transform.translation.x = turtle_pose.position.x
    t.transform.translation.y = turtle_pose.position.y
    t.transform.translation.z = turtle_pose.position.z
    t.transform.rotation.x = turtle_pose.orientation.x
    t.transform.rotation.y = turtle_pose.orientation.y
    t.transform.rotation.z = turtle_pose.orientation.z
    t.transform.rotation.w = turtle_pose.orientation.w

    # 广播tf
    br.sendTransform(t)

　　第七步，在循环中调用回调函数。

while not rospy.is_shutdown():
    try:
        # 监听海龟的相对于world的位置
        listener.waitForTransform("world", "turtle1", rospy.Time(0), rospy.Duration(1.0))

        # 调用回调函数
        callback(listener.lookupTransform("world", "turtle1", rospy.Time(0)))
    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        continue

    # 等待一段时间
    rate.sleep()