ROS 动态坐标变换

ROS 动态坐标变换

案例：实现小乌龟运动时，实时显示乌龟位姿与原点的相对关系，并能够在rviz中实时观察到两坐标的相对位置

实现思路：发布方订阅乌龟运动的位姿信息，建立乌龟（子级坐标系）与原点（父级坐标系）并发布相对关系，订阅方订阅到两坐标系并输出乌龟相对原点的动态变换坐标。

注意：这里的发布方订阅了内置节点乌龟运动的信息，其实是个嵌套实现

①发布方编码实现

#! /usr/bin/env python
import rospy
from turtlesim.msg import Pose
import tf2_ros
from geometry_msgs.msg import TransformStamped
import tf.transformations
"""
发布方实现：
订阅乌龟的位姿信息，转换成坐标系的相对关系并发布
准备：了解乌龟位姿的话题和消息类型：话题是turtle1/pose，类型turtlesim/Pose
流程：
1、导包
2、初始化ros节点
3、创建订阅对象
4、回调函数处理订阅到的消息（核心）
5、spin（）
"""
def doPose(pose):
    #创建发布坐标系相对关系的对象
    pub = tf2_ros.TransformBroadcaster()
    #将Pose转换成坐标系相对关系的消息
    ts = TransformStamped()
    ts.header.frame_id = "world"
    ts.header.stamp = rospy.Time.now()
    ts.child_frame_id = "turtle1"
    #transform,坐标系子级坐标系相对于父级坐标系的偏移量
    ts.transform.translation.x = pose.x
    ts.transform.translation.y = pose.y
    ts.transform.translation.z = 0
    #ts下的rotation，通过四元数设置方向上的偏转角（一般需要一个欧拉角的转换过程）
    """
    乌龟是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]
    #发布
    pub.sendTransform(ts)   

if __name__=="__main__":
    rospy.init_node("dynamic_pub")
    #创建订阅对象
    sub = rospy.Subscriber("/turtle1/pose",Pose,doPose,queue_size = 100)
    rospy.spin()

②订阅方编码实现

  这里要格外注意时间戳要置0，采用rospy.Time()，如果仍然采用rospy.Time.now获取当前，坐标转换会因为时间戳相差较大而无法完成转换从而抛出异常。

#! /usr/bin/env python
import rospy
import tf2_ros
from tf2_geometry_msgs import tf2_geometry_msgs
"""
订阅方实现：
订阅坐标变换消息，传入被转换的坐标点，调用转换算法实现坐标转换
1、导包（用到什么包导入什么包）
2、初始化ros节点
3、创建订阅对象
4、组织被转换的坐标点
5、转换逻辑编写（调用tf封装算法）
6、输出转换结果B
7、spin（）/spinonce（）
"""

if __name__=="__main__":

    #初始化ros节点
    rospy.init_node("static_tf_pub")
    #创建订阅对象
    buffer = tf2_ros.Buffer() #创建缓存对象
    sub = tf2_ros.TransformListener(buffer)#将缓存对象传入
    #组织被转换的坐标点
    ps = tf2_geometry_msgs.PointStamped()
    #时间戳 0 
    ps.header.stamp = rospy.Time()
    ps.header.frame_id = "turtle1"
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        try:
            #转换：buffer提供了对应的转换函数
            ps_out = buffer.transform(ps,"world")
            rospy.loginfo("转换后的坐标结果:(%.2f,%.2f,%.2f)当前参考的坐标系：%s",ps_out.point.x,ps_out.point.y,ps_out.point.z,ps_out.header.frame_id) 
        except:
            rospy.logwarn("错误提示：%s",Exception)
        rate.sleep()

③运行

  启动roscore，启动乌龟GUI节点和键盘控制节点，分别rosrun发布方和订阅方，打开rviz，可以看到键盘控制乌龟运动时，会实时输出乌龟相对原点坐标的位置信息和坐标的动态变化图