TF坐标变换实操

对于TF坐标变换的实操

#! /usr/bin/env python

import rospy
from turtlesim.srv import Spawn, SpawnRequest, SpawnResponse


if __name__ == "__main__":
    rospy.init_node("service_call_p")
    client = rospy.ServiceProxy("/spawn", Spawn)
    request = SpawnRequest()
    request.x = 1.0
    request.y = 1.0
    request.theta = 3.14
    request.name = "turtle2"
    client.wait_for_service()
    try:
        response = client.call(request)
        rospy.loginfo("the turtle name is %s", response.name)
    except Exception as e:
        rospy.logerr("error %s", e)
   

#! /usr/bin/env python

import rospy
import tf2_ros
import tf
from turtlesim.msg import Pose
from geometry_msgs.msg import TransformStamped
import sys


turtle_name = ""

def doPose(pose):

    broadcaster = tf2_ros.TransformBroadcaster()
 
    tfs = TransformStamped()
    tfs.header.frame_id = "world"
    tfs.header.stamp = rospy.Time.now()
    tfs.child_frame_id = turtle_name
    tfs.transform.translation.x = pose.x
    tfs.transform.translation.y = pose.y
    tfs.transform.translation.z = 0.0
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    tfs.transform.rotation.x = qtn[0]
    tfs.transform.rotation.y = qtn[1]
    tfs.transform.rotation.z = qtn[2]
    tfs.transform.rotation.w = qtn[3]
    broadcaster.sendTransform(tfs)

if __name__ == "__main__":

    rospy.init_node("dynamic_tf_pub_p")
    if len(sys.argv) != 4:
        rospy.loginfo("argv error")
        sys.exit(1)
    else:
	    # 文件全路径 + 传入的参数 + 节点名称 + 日志文件路径
        turtle_name = sys.argv[1]
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,doPose)
    rospy.spin()

turtle_name = sys.argv[1]
在你提供的 Python 脚本 test02_pub_turtle_p.py 中，sys.argv 的内容将包括以下元素：

1. sys.argv[0]: 脚本的名称，即脚本文件的路径。
2. sys.argv[1]: 第一个命令行参数，即传递给脚本的参数，对应于 turtle_name。
3. sys.argv[2]: 第二个命令行参数，对应于节点名称。
4. sys.argv[3]: 第三个命令行参数，对应于日志文件路径。

在 launch 文件中，<node> 标签使用了 args 属性将命令行参数传递给 test02_pub_turtle_p.py 脚本。具体的传递方式如下：

<node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen" />

在这个例子中，args="turtle1" 意味着将 “turtle1” 作为第一个命令行参数传递给 test02_pub_turtle_p.py 脚本。这对应于 sys.argv[1]。

#! /usr/bin/env python


import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped, Twist
import math


if __name__ == "__main__":
    rospy.init_node("static_sub_p")
    
    buffer = tf2_ros.Buffer()

    sub = tf2_ros.TransformListener(buffer)
    pub = rospy.Publisher("turtle2/cmd_vel", Twist, queue_size=100)
    rate = rospy.Rate(10)
   
    while not rospy.is_shutdown():
        try:
            ts = buffer.lookup_transform("turtle2" ,"turtle1", rospy.Time(0))

            rospy.loginfo("header:%s   child:%s %.2f %.2f %.2f",
                          ts.header.frame_id,
                          ts.child_frame_id,
                          ts.transform.translation.x,
                          ts.transform.translation.y,
                          ts.transform.translation.z)
            twist = Twist()
            # 线速度 = 系数 * （x^2 + y^2）再开方
			# 角速度 = 系数 * atan2（y,x） 
            twist.linear.x = 0.5 * math.sqrt(math.pow(ts.transform.translation.x,2)+math.pow(ts.transform.translation.y,2))
            twist.angular.z = 4 * math.atan2(ts.transform.translation.y, ts.transform.translation.x)

            pub.publish(twist)
        except Exception as e:2
            rospy.logwarn("%s", e)

        rate.sleep()

ts = buffer.lookup_transform(“turtle2” ,“turtle1”, rospy.Time(0))

在ROS中，rospy.Time(0) 是一个特殊的时间戳，表示"最新的可用时间戳"。使用这个时间戳，buffer.lookup_transform("turtle2", "turtle1", rospy.Time(0)) 将返回最新的turtle2到turtle1坐标系之间的变换。

<launch>
    <node pkg="turtlesim" type="turtlesim_node" name="turtle1" output="screen" />
    <node pkg="turtlesim" type="turtle_teleop_key" name="key" output="screen" />

    <node pkg="tf04_test" type="test01_new_turtle_p.py" name="turtle2" output="screen"  />
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub1" args="turtle1" output="screen"  />
    <node pkg="tf04_test" type="test02_pub_turtle_p.py" name="pub2" args="turtle2" output="screen"  />
    <node pkg="tf04_test" type="test03_control_turtle2_p.py" name="control" output="screen" />
</launch>