ROS 坐标变换实操(Python版)

一、建立launch文件 test_p.launch

<launch>
    <!-- 
        流程详解:
            1. 准备工作: 启动乌龟 GUI 节点与键盘控制节点
            2. 需要调用服务器生成一只新的乌龟
            3. 发布两只乌龟的坐标信息
            4. 订阅坐标信息，并且转换成 乌龟A 相对于 乌龟B 的坐标信息, 最后再生成控制乌龟B的速度信息
     -->
     <!-- 1. 准备工作: 启动乌龟 GUI 节点与键盘控制节点 -->
    <node pkg = "turtlesim" type = "turtlesim_node" name = "turtle1" output = "screen"/>
    <!-- 键盘控制-->
    <node pkg = "turtlesim" type = "turtle_teleop_key" name = "key" output = "screen" />

    <!-- 2. 需要调用服务器生成一只新的乌龟 -->
    <node pkg = "tf04_test" type = "test01_new_turtle_p.py" name = "turtle2" output = "screen" />

    <!-- 3. 发布两只乌龟的坐标信息:
            (1) 复用之前的乌龟坐标发布功能
            (2) 调用节点时，以参数的方式传递乌龟名称，解析参数置换: 订阅的话题消息 和 子级坐标系的名称
            args表示传参，args[0]表示函数名称，args[1]才表示传入的第一个参数
    -->
    <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" />

    <!-- 4. 订阅坐标信息，并且转换成 乌龟A 相对于 乌龟B 的坐标信息, 最后再生成控制乌龟B的速度信息 -->
    <node pkg = "tf04_test" type = "test03_control_turtle2_p.py" name = "control" output = "screen"/>

</launch>

二、生成一个新的乌龟节点 test01_new_turtle_p.py

launch文件

 <!-- 2. 需要调用服务器生成一只新的乌龟 -->
    <node pkg = "tf04_test" type = "test01_new_turtle_p.py" name = "turtle2" output = "screen" />

#! /usr/bin/env python


"""
    需求：向服务器发送请求生成一个乌龟
        话题：/spawn
        消息类型：turtlesim/Spawn
    1. 导包
    2. 初始化ROS节点
    3. 创建服务的客户端对象
    4. 组织数据并发送请求
    5. 处理响应结果

"""

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

if __name__ == "__main__":
    # 2. 初始化ROS节点
    rospy.init_node("service_call_p")
    # 3. 创建服务的客户端对象
    client = rospy.ServiceProxy("/spawn",Spawn) # /spawn是话题名称
    # 4. 组织数据并发送请求
    # 4.1 组织数据
    request = SpawnRequest()
    request.x = 4.5
    request.y = 2.0
    request.theta = -3 # 向右转3个rad
    request.name = "turtle2"
    # 4.2 判断服务器状态并发送
    client.wait_for_service() # 服务端开启则执行，否则挂起
    # 防止抛出异常
    try:
        response = client.call(request)
        # 5. 处理响应结果
        rospy.loginfo("生成乌龟的名字叫：%s",response.name)
    except Exception as e:
        rospy.logerr("请求处理异常")

rostopic list

/rosout
/rosout_agg
/turtle1/cmd_vel (目前只能控制)
/turtle1/color_sensor
/turtle1/pose
/turtle2/cmd_vel
/turtle2/color_sensor
/turtle2/pose

发布方(发布两只相对于世界坐标系的乌龟 )

test02_pub_turtle_p.py

乌龟的坐标信息是相对于世界坐标系而言的
launch 文件

<!-- 3. 发布两只乌龟的坐标信息:
            (1) 复用之前的乌龟坐标发布功能
            (2) 调用节点时，以参数的方式传递乌龟名称，解析参数置换: 订阅的话题消息 和 子级坐标系的名称
            args表示传参，args[0]表示函数名称，args[1]才表示传入的第一个参数
    -->
    <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" />

#! /usr/bin/env python

"""
    发布方: 需要订阅乌龟的位姿信息, 转换成相对于窗体的坐标关系，并发布
    准  备: 
        话题： /turtle1/pose
        消息： turtlesim/Pose

    流程:
        1. 包含头文件
        2. 初始化ROS节点
        3. 创建订阅对象，订阅/turtle1/pose
        4. 回调函数处理订阅信息: 将位姿信息转换成坐标相对关系并发布(关注)
        5. spin()
"""

import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import sys # 解析参数

# 接受乌龟名称的变量
turtle_name = " "


def doPose(pose):
    # 1. 创建发布坐标系相对关系的对象 头文件3
    pub = tf2_ros.TransformBroadcaster()

    # 2. 将 pose 转换成 坐标系相对关系消息 头文件4
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()

    # 修改1 --------------------------- 子级坐标系
    ts.child_frame_id = turtle_name
    
    # 2.1 子级坐标系相对于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0

    # 2.2 四元数 头文件5
    # 从欧拉角转换四元数
    """
        乌龟是2D，不存在 x 上的翻滚和 y 上的俯仰，只有 z 上的偏航
        0 0 pose.theta
    """
    qtn = tf.transformations.quaternion_from_euler(0,0,pose.theta)
    ts.transform.rotation.x = qtn[0]
    ts.transform.rotation.y = qtn[1]
    ts.transform.rotation.z = qtn[2]
    ts.transform.rotation.w = qtn[3]

    # 3. 发布
    pub.sendTransform(ts)

if __name__ == "__main__":

    # 2. 初始化ROS节点
    rospy.init_node("dynamic_pub_p")

    # 解析传入的参数(现在传入几个参数? 文件全路径 + 传入的参数 + 节点名称 + 日志文件路径)
    if len(sys.argv) != 4:
        rospy.loginfo("参数个数不对")
        sys.exit(1)
    else:
        turtle_name = sys.argv[1] # 这里argv[1]指的就是turtle1或者turtle2

    # 3. 创建订阅对象 头文件2

    # 修改2 -------------------- 乌龟的名称是通过参数传递进来的
    sub = rospy.Subscriber(turtle_name + "/pose",Pose,doPose,queue_size = 100) # /tur是话题名称

    # 4. 回调函数处理订阅信息
    # 5. spin()
    rospy.spin()

关键代码

# 解析传入的参数(现在传入几个参数? 文件全路径 + 传入的参数 + 节点名称 + 日志文件路径)
    if len(sys.argv) != 4:
        rospy.loginfo("参数个数不对")
        sys.exit(1)
    else:
        turtle_name = sys.argv[1] # 这里argv[1]指的就是turtle1或者turtle2

订阅方(解析坐标信息)

launch文件

<!-- 4. 订阅坐标信息，并且转换成 乌龟A 相对于 乌龟B 的坐标信息, 最后再生成控制乌龟B的速度信息 -->
    <node pkg = "tf04_test" type = "test03_control_turtle2_p.py" name = "control" output = "screen"/>

#! /usr/bin/env python

# 实现功能: 1 坐标1向坐标2转变 2 坐标1中点在坐标2的变换


import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
from geometry_msgs.msg import TransformStamped

if __name__ == "__main__":

    # 2. 初始化
    rospy.init_node("tfs_sub_p")

    # 3. 创建订阅对象
    # 3.1 创建一个缓存对象 头文件2
    buffer = tf2_ros.Buffer()
    # 3.2 创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    # 4. 组织被转换的坐标点 头文件3(这里不需要)

    # 5. 转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # ------ 需要计算 son1 相对于 son2 的坐标关系  头文件4
            """
                参数1: 目标坐标系
                参数2: 源坐标系
                参数3: rospy.Time(0) --- 取时间间隔最近的两个坐标系帧(son1相对world,与son2相对world)计算
                返回值: turtle1 与 turtle2坐标系关系
            """
            ts = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0)) # 目标坐标系， 源坐标系
            rospy.loginfo("父级坐标系: %s,子级坐标系: %s, 偏移量(%.2f,%.2f,%.2f)",
                        ts.header.frame_id,
                        ts.child_frame_id,
                        ts.transform.translation.x,
                        ts.transform.translation.y,
                        ts.transform.translation.z
                        )

        except Exception as e:
            rospy.logwarn("错误提示:%s",e) 
        
        rate.sleep()

订阅方(生成速度信息并发布)

#! /usr/bin/env python

# 实现功能: 1 坐标1向坐标2转变 2 坐标1中点在坐标2的变换


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__":

    # 2. 初始化
    rospy.init_node("tfs_sub_p")

    # 3. 创建订阅对象
    # 3.1 创建一个缓存对象 头文件2
    buffer = tf2_ros.Buffer()
    # 3.2 创建订阅对象(将缓存传入)
    sub = tf2_ros.TransformListener(buffer)

    # 4. 创建速度消息发布对象
    pub = rospy.Publisher("/turtle2/cmd_vel",Twist,queue_size=100)

    # 5. 转换逻辑实现，调用tf封装的算法
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        # 使用try防止
        try:
            # ------ 需要计算 son1 相对于 son2 的坐标关系  头文件4
            """
                参数1: 目标坐标系
                参数2: 源坐标系
                参数3: rospy.Time(0) --- 取时间间隔最近的两个坐标系帧(son1相对world,与son2相对world)计算
                返回值: turtle1 与 turtle2坐标系关系
            """
            ts = buffer.lookup_transform("turtle2","turtle1",rospy.Time(0)) # 目标坐标系， 源坐标系
            rospy.loginfo("父级坐标系: %s,子级坐标系: %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 消息
            # 组织速度,只需要设置线速度的 x 与 角速度的 z
            # x = 系数 * sqrt((y^2+x^2))
            # z = 系数 * atan2(对边/邻边) 反正切，以弧度为单位
            twist = Twist()
            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:
            rospy.logwarn("错误提示:%s",e) 
        
        rate.sleep()

重点

 # 组织 Twist 消息
            # 组织速度,只需要设置线速度的 x 与 角速度的 z
            # x = 系数 * sqrt((y^2+x^2))
            # z = 系数 * atan2(对边/邻边) 反正切，以弧度为单位
            twist = Twist()
            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)