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,可以看到键盘控制乌龟运动时,会实时输出乌龟相对原点坐标的位置信息和坐标的动态变化图