[ROS] TF2 좌표변환 및 실례

헤일로 뿌아입니다. 보통은 주로 C++, 데이터 구조 알고리즘을 업데이트하는데... 관심있으신 분들은 저를 팔로우 해주세요! 당신은 실망하지 않을 것입니다.

0. ROS에서 Coordinate 변환 메시지 패키지

일상 생활에서, 특히 로봇의 경우 각 대상 시스템의 좌표 변환은 매우 중요하며 좌표는 오른손 시스템으로 표시됩니다.
ROS에서 좌표 변환을 제공하는 소프트웨어 패키지인 Transform Frame TF 의 기능은 ROS에서 다른 좌표점/벡터의 변환을 실현하는 것입니다.

그러나 TF는 이전에 여러 버전에서 더 이상 사용되지 않았으며 이제 여러 관련 기능 패키지가 있는 새 버전인 TF2가 사용됩니다.

1. tf2_geometry_msgs: ROS 메시지를 tf2 메시지로 변환할 수 있습니다.
2. tf2: 좌표 변환을 위한 공통 메시지를 캡슐화합니다.
3. tf2_ros: tf2에 대한 roscpp 및 rospy 바인딩을 제공하고 좌표 변환을 위한 공통 API를 캡슐화합니다.
   

좌표계 변환에서 지오메트리 아래에는 두 가지 중요한 메시지 유형이 있습니다. TransformStamped, PointStamped 전자는 좌표계 간 변환에 사용되며 후자는 점 간 좌표 변환에 사용되며 이는 향후 사용에 매우 중요합니다. 먼저 이 두 가지 메시지 유형의 내용을 이해해 봅시다.

0.1 geometry_msgs/TransformStamped

이 메시지 유형은 좌표계 간의 관계를 나타냅니다.
터미널에 입력하십시오.

rosmsg info geometry_msgs/TransformStamped

이 메시지 유형에 대한 특정 정보 보기:

std_msgs/Header header  # 头信息
    uint32 seq          ## 序列号 
    time stamp          ## 时间戳
    string frame_id     ## 坐标

string child_frame_id   # 子坐标

geometry_msgs/Transform transform   #坐标信息

    geometry_msgs/Vector3 translation ##偏移量

        float64 x
        float64 y
        float64 z

    geometry_msgs/Quaternion rotation #四元数(欧拉角)

        float64 x
        float64 y
        float64 z
        float64 w

PointStamped 메시지는 함께 캡슐화된
std_msgs/Header,string , geometry_msgs/Transform 으로 구성된 새로운 메시지 유형임을 알 수 있습니다 .
그 중 Transform은 geometry_msgs/Vector3, geometry_msgs/Quaternion 으로 캡슐화 됩니다.

0.2 geometry_msgs/PointStamped

이 메시지 유형은 좌표점 간의 변환을 나타냅니다.
터미널에 입력

rosmsg info geometry_msgs/PointStamped

메시지에서 특정 정보를 볼 수 있습니다.

std_msgs/Header header      #头信息
    uint32 seq                  ##序列号
    time stamp                  ##时间戳
    string frame_id             ##坐标系
geometry_msgs/Point point   #点坐标
    float64 x                   
    float64 y
    float64 z

PointStamped 메시지는 함께 캡슐화된 std_msgs/Header 및 geometry_msgs/Point 로 구성된 새로운 메시지 유형임을 알 수 있습니다 .

1. 정적 좌표 변환

기존의 로봇 모델이 존재하며, 핵심 구조는 본체와 레이더를 포함하며, 각각 좌표계에 해당하며, 좌표계의 원점은 각각 본체와 레이더의 물리적 중심에 위치한다. 레이더 원점과 본체 원점 사이는 x 0.2 y0.0 z0.5. 현재 레이더에서 장애물을 감지한 경우 레이더 좌표계에서 장애물의 좌표는 (2.0 3.0 5.0)입니다. 물체에 대한 장애물의 좌표는 얼마입니까?

게시자의 전체 논리 구성:

1. 필수 기능 패키지 가져오기
2. ros 노드 초기화
3. 정적 좌표 브로드캐스터 만들기
4. 정적 좌표 정보 쓰기
5. 메세지를 보내다
6. 회전()

수신자 논리는 다음과 같습니다.

1. 필요한 기능 패키지 가져오기
2. ros 노드 초기화
3. TF 구독 개체 만들기
4. LASE의 좌표점 생성
5. 좌표 변환
6. 회전()

1.1 필요한 기능 패키지 가져오기

이 경우 rospy와 std_msgs의 두 가지 표준 구성 요소가
필요합니다.또한
tf2: 좌표 변환을 위한 공통 메시지를 캡슐화합니다.
tf2_ros: tf2에 대한 roscpp 및 rospy 바인딩을 제공하고 좌표 변환을 위한 공통 API를 캡슐화합니다.
tf2_geometry_msgs: ROS 메시지를 tf2 메시지로 변환할 수 있습니다.

1.2 게시자 구현

"""
导入功能包
""" 
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf
import rospy
"""
初始化节点信息
创建发布对象
组织发布数据
发布数据
spin()
"""
#初始化ros节点
rospy.init_node("static_pub")
#创建静态发布对象
pub=tf2_ros.StaticTransformBroadcaster()
#组织消息类型
ts=TransformStamped()

ts.header.seq=123
ts.header.stamp=rospy.Time.now()
ts.child_frame_id="laser"
ts.header.frame_id="frame_id"
ts.transform.translation.x=0.2
ts.transform.translation.y=0
ts.transform.translation.z=0.5
"""
将欧拉角放到四元数中进行转换
用到了tf中的transformation.quaternion_from_euler
"""
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(tf)
rospy.spin()

1.3 가입자 구현

"""
导入功能包
"""
import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros

#初始化节点
rospy.init_node("static_sub")

#创建缓存对象
buffer=tf2_ros.Buffer()
"""

调用tf2_ros.Buffer()创建一个buffer用来存储坐标消息

"""
tf2_ros.TransformListener(buffer)
"""

监听tf坐标变换,将值存入buffer中

"""

"""

创建点坐标信息

"""
ps=tf2_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.transform 将点坐标与原始坐标进行转换
        
        """
        ps_out=buffer.transform(ps,"frame_id")

        rospy.loginfo("转换后的坐标：（%.2f,%.2f,%.2f）,参考坐标系：%s",
                    ps_out.point.x,
                    ps_out.point.y,
                    ps_out.point.z,
                    ps_out.header.frame_id)
    except Exception as ee:
        rospy.logwarn("错误提示%s",ee)
    rate.sleep()

1.4 tf2_ros는 정적 좌표 변환을 구현합니다.

정적 좌표 변환의 전체적인 로직은 대략 동일하므로 tf2_ros는 매번 코드를 작성하지 않고 좌표 변환을 직접 구현할 수 있는 함수 패키지를 제공합니다.

rosrun tf2_ros static_transform_publisher x偏移量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系

2. 동적 좌표 변환

실생활에서 우리는 point-to-point 좌표 변환뿐만 아니라 동적 좌표 변환에 직면합니다.
동적 좌표 변환을 구현하기 위해 거북이를 예로 들어 보겠습니다.

퍼블리셔의 논리를 먼저 정리하자

1. rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim
2. ros 노드 초기화
3. /turtle1/pose 주제 뉴스 구독
4. 콜백
   1. TF 브로드캐스터 만들기
   2. 방송 데이터 정리
   3. 브로드캐스터가 데이터를 게시합니다.
5. 스핀
   리시버 의 논리
6. 가이드 패키지
7. ros 노드 초기화
8. TF 객체 생성
9. 구독 데이터 처리

2.1 게시자 구현

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

订阅乌龟的位姿信息

"""
def doPose(pose):
    #创建动态坐标发布对象
    pub=tf2_ros.TransformBroadcaster()
    #组织点坐标消息类型
    ts=TransformStamped()

    ts.header.frame_id="world"

    ts.child_frame_id="turtle1"

    ts.header.stamp=rospy.Time.now()

    #坐标系相对于子集坐标系

    ts.transform.translation.x=pose.x
    ts.transform.translation.y=pose.y
    ts.transform.translation.z=0
    
    #四元数转换
    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]
    pub.sendTransform(ts)

#初始化ROS节点
rospy.init_node("tf02_pub")
#订阅消息位姿信息，创建回调函数
sub=rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size=100)
rospy.spin()

2.2 가입자 로직

import rospy
import tf2_ros
# 不要使用 geometry_msgs,需要使用 tf2 内置的消息类型
from tf2_geometry_msgs import PointStamped
# from geometry_msgs.msg import PointStamped

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("static_sub_tf_p")
    # 3.创建 TF 订阅对象
    buffer = tf2_ros.Buffer()
    # 监听坐标变换存入buffer中
    tf2_ros.TransformListener(buffer)

    rate = rospy.Rate(1)
    while not rospy.is_shutdown():    
    # 4.创建坐标点信息
        # 仅需提供目标坐标系
        point_source = PointStamped()

        point_source.header.frame_id = "turtle1"
        
        point_source.header.stamp = rospy.Time.now()
        


        try:
    #     5.调研订阅对象的 API 将 4 中的点坐标转换成相对于 world 的坐标
            point_target = buffer.transform(point_source,"world",rospy.Duration(1))
            rospy.loginfo("转换结果:x = %.2f, y = %.2f, z = %.2f",
                            point_target.point.x,
                            point_target.point.y,
                            point_target.point.z)
        except Exception as e:
            rospy.logerr("异常:%s",e)

    #     6.spin
        rate.sleep()

2.3 효과 실현

먼저 Turtlesim의 키보드 제어 노드와 GUI를 시작합니다.

rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key

발행자와 수신자를 시작하면 화면에서 변환된 좌표계를 볼 수 있습니다.

rosrun tf02_dynamic demo01_tf02_pub.py
rosrun tf02_dynamic demo01_tf02_sub.py

3.0 다좌표 변환

세계 좌표계를 기준으로 여러 좌표를 먼저 변환한 후 api를 호출하여 변환된 데이터를 서로 변환

3.1 게시자 구현

정적 좌표 변환의 ros 패키지를 직접 호출하여 실행 파일로 작성

<launch>
    <node pkg="tf2_ros" type="static_transform_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" />
    <node pkg="tf2_ros" type="static_transform_publisher" name="son2"
    args="0.5 0 0 0 0 0 /world /son2"
    output="screen"/>
</launch>

3.2 가입자 구현

가입자 로직 구현

1. 가져오기 패키지 rospy std_msgs tf2_ros geometry_msgs(TransformStamped 좌표계 변환) tf2_geomerty_msgs(PointStamped 좌표점 변환)
2. ros 노드 초기화
3. 두 좌표계 간의 상호 변환을 실현하기 위한 TF 구독 객체 생성

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

rospy.init_node("static_sub")

#创建缓存对象
buffer=tf2_ros.Buffer()

sub=tf2_ros.TransformListener(buffer)

rate=rospy.Rate(10)

while not rospy.is_shutdown():
    try:
        """
        计算son1相对于son2的坐标关系
        lookup_transform(父级坐标系，子级坐标系，取坐标的时间，时间间隔)
        """
        ts=buffer.lookup_transform("son2","son1",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 ee:
        pass
        rospy.logwarn("错误提示%s",ee)
    rate.sleep()

3.3 현재 좌표계를 보기 위한 view_frames

위의 노드를 실행한 후 작업 디렉토리에 입력하십시오.

rosrun tf2_tools view_frames.py

좌표 관계가 있는 PDF가 현재 디렉토리에 생성됩니다. 이 도구를 사용하여 좌표 관계를 볼 수 있습니다.
그림 설명을 추가하십시오.

4.0 tf 좌표 변환의 실제 연산

먼저 거북이를 만들고 Turtlesim 노드를 실행해 봅시다.

rosrun turtlesim turtlesim_node

rosservice의 /spawn 서비스를 사용하여 거북이를 하나 더 생성하여 다중 좌표 변환을 완료하고 H라는 거북이를 생성합니다.

rosservice call /spawn 
"x: 0.0 
y: 0.0
theta: 0.0
name: ''" 

입력한 이름이 반환되면 이때 방금 생성된 거북이를 화면에 볼 수 있습니다.
준비가 완료되었으며 이제 좌표계 생성을 시작합니다.

4.1 거북이 포즈 정보 공개

먼저 다음 전체의 논리를 명확히 합시다.

1. 두 거북이의 정보를 좌표계에 게시
2. 첫 번째 거북이를 기준으로 두 번째 거북이의 자세 정보 변환
3. 제어 cmd 릴리스 속도 정보

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


def doPose(pose):
    pub=tf2_ros.TransformBroadcaster()
    ts=TransformStamped()
    ts.header.frame_id="world"
    ts.header.stamp=rospy.Time.now()
    ts.child_frame_id=turtle_name
    ts.transform.translation.x=pose.x
    ts.transform.translation.y=pose.y

    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]

    pub.sendTransform(ts)

rospy.init_node("dynamic_pub",anonymous=True)
if len(sys.argv)>=2: 
    turtle_name=sys.argv[1]
    sub=rospy.Subscriber(turtle_name+"/pose",Pose,doPose,queue_size=10)
    rospy.spin()
else:
    print(sys.argv[1])
    rospy.loginfo("请输入坐标名称")
    sys.exit()

이 코드는 여러 번 등장하므로 여기서는 반복하지 않겠습니다. 참고: sys.argv의 첫 번째 매개변수는 파일 이름 다음에 들어오는 매개변수입니다.

4.2 거북이 움직임을 따르도록 제어

전반적인 논리:

1. 두 거북 사이의 상대 좌표 계산
2. 거북이의 선속도와 각속도 제어
3. 풀어 주다

import rospy
from tf2_geometry_msgs import tf2_geometry_msgs
import tf2_ros
from geometry_msgs.msg import TransformStamped,Twist
import math
import sys
"""
创建订阅对象
组织被转换的坐标点
转换逻辑实现调用tf封装的算法
输出结果
"""
rospy.init_node("static_sub")

#创建缓存对象
buffer=tf2_ros.Buffer()

sub=tf2_ros.TransformListener(buffer)
pub=rospy.Publisher("/H/cmd_vel",Twist,queue_size=10)
rate=rospy.Rate(10)
while not rospy.is_shutdown():
    try:
        """
        计算son1相对于son2的坐标关系
        直接监听整个坐标系，不需要订阅话题
        """
        ts=buffer.lookup_transform("H","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=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 ee:
        pass
        rospy.logwarn("错误提示%s",ee)
    rate.sleep()

4.3 현재 좌표 관계 보기

rosrun tf2_tools view_frames.py