Halo,这里是Ppeua。平时主要更新C++,数据结构算法…感兴趣就关注我吧!你定不会失望。
0.ROS中的坐标转换消息包0.1 geometry_msgs/TransformStamped0SEO靠我.2 geometry_msgs/PointStamped1.静态坐标转换1.1导入所需功能包1.2发布方实现1.3 订阅方实现1.4 tf2_ros实现静态坐标转换2.动态坐标转换2.1发布方实现2SEO靠我.2订阅方逻辑2.3实现效果3.0多坐标转换3.1发布方实现3.2订阅方实现3.3 view_frames查看当前坐标系4.0 tf坐标变换实操4.1乌龟位姿信息发布4.2 控制乌龟进行跟随运动4.3SEO靠我查看当前坐标关系
在日常生活中,特别是对于机器人来说,各个目标系中的坐标转换是很关键的,通过右手系来标注坐标。
ROS中提供了坐标转换的软件包 Transform FrameTFSEO靠我的作用是ROS中实现不同坐标点/向量的转换。
不过TF在若干个版本前已经弃用,现在使用的是全新的版本:TF2,其有几个相关的功能包:
tf2_geometry_msgs:可以将ROS消息转换成tf2消息。SEO靠我tf2: 封装了坐标变换的常用消息。tf2_ros:为tf2提供了roscpp和rospy绑定,封装了坐标变换常用的API。在坐标系转换中,在geometry下有两个重要的消息类型:TransformSSEO靠我tamped、PointStamped,前者用于坐标系间的转换,后者用于点之间的坐标转换,这对我们之后的使用很重要。先来了解下这两种消息类型中的内容。
该消息类型表示坐标系之间的关系 在终端中输入
rosmsg info geometry_msgs/TransformStamped查看该消息类型的具体信息:
std_msgs/HeaderSEO靠我 header # 头信息uint32 seq ## 序列号 time stamp ## 时间戳string frame_id ## 坐标string child_frame_id # 子坐标geomSEO靠我etry_msgs/Transform transform #坐标信息geometry_msgs/Vector3 translation ##偏移量float64 xfloat64 yfloat64 SEO靠我zgeometry_msgs/Quaternion rotation #四元数(欧拉角)float64 xfloat64 yfloat64 zfloat64 w可以看出 PointStamped消息是SEO靠我由:
std_msgs/Header,string,geometry_msgs/Transform封装在一起,组成的新消息类型。
其中Transform又是由geometry_msgs/Vector3,gSEO靠我eometry_msgs/Quaternion进行封装的。该消息类型表示坐标点之间的转换 在终端中输入
rosmsg info geometrySEO靠我_msgs/PointStamped可以查看该消息中的具体信息
std_msgs/Header header #头信息uint32 seq ##序列号time stamp ##时间戳string fraSEO靠我me_id ##坐标系 geometry_msgs/Point point #点坐标float64 x float64 yfloat64 z可以看出 PointStamped消息是由:SEO靠我
std_msgs/Header与geometry_msgs/Point封装在一起,组成的新消息类型。现有一机器人模型,核心构成包含主体与雷达,各对应一坐标系,坐标系的原点分别位于主体与SEO靠我雷达的物理中心,已知雷达原点相对于主体原点位移关系如下: x 0.2 y0.0 z0.5。当前雷达检测到一障碍物,在雷达坐标系中障碍物的坐标为 (2.0 3.0 5.0),请问,该障碍物相对于主体的坐SEO靠我标是多少?
组织下我们发布方的整体逻辑:
导入所需功能包初始化ros节点创建静态坐标广播器编写静态坐标信息发送消息spin()这里是接收方的逻辑:
导入所需要的功能包初始化ros节点创建TF订阅对象创建laSEO靠我se的坐标点坐标转换spin()在这个案例中,需要:rospy,std_msgs
这两个标准件
还需要:
tf2:封装了坐标变换的常用消息。 tf2_ros: 为tf2提供了roscpp和rSEO靠我ospy绑定,封装了坐标变换常用的API。 tf2_geometry_msgs:可以将ROS消息转换成tf2消息。由于静SEO靠我态坐标转换中的整体逻辑大致相同,所以tf2_ros提供了一个功能包来直接实现坐标转换,不需要每次都使用编写代码
rosrun tf2_ros static_transform_publisher x偏移SEO靠我量 y偏移量 z偏移量 z偏航角度 y俯仰角度 x翻滚角度 父级坐标系 子级坐标系在现实生活中,我们面对的不仅有点对点的坐标转换,还动态的坐标转换。
我们以乌龟为例来实现一下动态坐标转换SEO靠我先来组织下发布方的逻辑
导包 rospy std_msgs tf2 tf2_ros tf2_geometry_msgs geometry_msgs turtlesim初始化ros节点订阅 /turtleSEO靠我1/pose 话题消息回调函数 创建TF广播器组织广播数据广播器发布数据 spin
接收方的逻辑导包初始化ros节点创建TF对象处理订阅数据首先启动turtlesim的键盘控制节点与GUI
rosrun turtlesim turtSEO靠我lesim_node rosrun turtlesim turtle_teleop_key接着启动发布方与接收方 之后就可以在屏幕上看到转换后的坐标系
rosrun tf02_dynamSEO靠我ic demo01_tf02_pub.py rosrun tf02_dynamic demo01_tf02_sub.py将多个坐标先相对于世界坐标系进行转换,然后在调用SEO靠我api将转换后的数据进行相互转换
直接调用静态坐标转换的ros包,写成launch文件
<launch><node pkg="tf2_ros" type="static_transformSEO靠我_publisher" name="son1" args="0.2 0.8 0.3 0 0 0 /world /son1" output="screen" /><node pkg="tf2_ros" SEO靠我type="static_transform_publisher" name="son2"args="0.5 0 0 0 0 0 /world /son2"output="screen"/> SEO靠我 </launch>订阅方逻辑实现
导入包 rospy std_msgs tf2_ros geometry_msgs(TransformStamped 坐标系转换) tf2_geoSEO靠我merty_msgs(PointStamped 坐标点转换)初始化ros节点创建TF订阅对象,实现两个坐标系之间相互转换 import rospy from tf2_SEO靠我geometry_msgs import tf2_geometry_msgs import tf2_ros from geometry_msgs.msg import SEO靠我TransformStampedrospy.init_node("static_sub")#创建缓存对象 buffer=tf2_ros.Buffer()sub=tf2_ros.TranSEO靠我sformListener(buffer)rate=rospy.Rate(10)while not rospy.is_shutdown():try:"""计算son1相对于son2的坐标关系lookuSEO靠我p_transform(父级坐标系,子级坐标系,取坐标的时间,时间间隔)"""ts=buffer.lookup_transform("son2","son1",rospy.Time(0))rospy.SEO靠我loginfo("父级坐标系:%s,子级坐标系:%s,%.2f,%.2f,%.2f",ts.header.frame_id,ts.child_frame_id,ts.transform.translaSEO靠我tion.x,ts.transform.translation.y,ts.transform.translation.z)except Exception as ee:passrospy.logwarSEO靠我n("错误提示%s",ee)rate.sleep()运行以上节点后,在任意工作目录下输入
rosrun tf2_tools view_frames.py会在当SEO靠我前目录下生成一个可以坐标关系的pdf,可以利用此工具查看坐标关系 请添加图片描述
我们先来创建turtle,运行turtlesim这个节点
rosrun turtlesim turtSEO靠我lesim_node通过rosservice的/spawn服务来多生成一只turtle来完成我们的多坐标转换,生成一只名为H的乌龟
rosservice call /spawn "x: SEO靠我0.0 y: 0.0 theta: 0.0 name: "若返回输入的名字,此时就能在屏幕上看到刚刚生成的那只乌龟
准备工作都做完了,现在开始创建坐标系
先来理清整个跟随的逻辑:
在坐标系中发布两只乌龟的信息将第二只乌龟的位姿信息相对第一只乌龟作转换控制cmd发布速度信息 import rospy imSEO靠我port sys from turtlesim.msg import Pose import tf2_ros from geometry_msgs.msSEO靠我g import TransformStamped import tfdef doPose(pose):pub=tf2_ros.TransformBroadcaster()ts=TraSEO靠我nsformStamped()ts.header.frame_id="world"ts.header.stamp=rospy.Time.now()ts.child_frame_id=turtle_naSEO靠我mets.transform.translation.x=pose.xts.transform.translation.y=pose.yqtn=tf.transformations.quaternioSEO靠我n_from_euler(0,0,pose.theta)ts.transform.rotation.x=qtn[0]ts.transform.rotation.y=qtn[1]ts.transformSEO靠我.rotation.z=qtn[2]ts.transform.rotation.w=qtn[3]pub.sendTransform(ts)rospy.init_node("dynamic_pub",aSEO靠我nonymous=True) if len(sys.argv)>=2: turtle_name=sys.argv[1]sub=rospy.Subscriber(turtle_name+SEO靠我"/pose",Pose,doPose,queue_size=10)rospy.spin() else:print(sys.argv[1])rospy.loginfo("请输入坐标名称SEO靠我")sys.exit()这份代码出现过很多次了,这里就不过多赘述。注意:sys.argv的第一个参数为文件名 之后的为传入参数
总体逻辑:
计算两个乌龟之间的相对坐标控制乌龟的SEO靠我线速度与角速度发布 import rospy from tf2_geometry_msgs import tf2_geometry_msgs imporSEO靠我t tf2_ros from geometry_msgs.msg import TransformStamped,Twist import math iSEO靠我mport sys """ 创建订阅对象 组织被转换的坐标点 转换逻辑实现调用tf封装的算法 输出结果 SEO靠我""" rospy.init_node("static_sub")#创建缓存对象 buffer=tf2_ros.Buffer()sub=tf2_ros.TransforSEO靠我mListener(buffer) pub=rospy.Publisher("/H/cmd_vel",Twist,queue_size=10) rate=rospy.RSEO靠我ate(10) while not rospy.is_shutdown():try:"""计算son1相对于son2的坐标关系直接监听整个坐标系,不需要订阅话题"""ts=bufferSEO靠我.lookup_transform("H","turtle1",rospy.Time(0))rospy.loginfo("父级坐标系:%s,子级坐标系:%s,%.2f,%.2f,%.2f",ts.heSEO靠我ader.frame_id,ts.child_frame_id,ts.transform.translation.x,ts.transform.translation.y,ts.transform.tSEO靠我ranslation.z)twist=Twist()twist.linear.x=0.5*math.sqrt(math.pow(ts.transform.translation.x,2)+math.pSEO靠我ow(ts.transform.translation.y,2))twist.angular.z=4*math.atan2(ts.transform.translation.y,ts.transforSEO靠我m.translation.x)pub.publish(twist)except Exception as ee:passrospy.logwarn("错误提示%s",ee)rate.sleep()网站备案号:浙ICP备17034767号-2