ros 学习（3）

最新推荐文章于 2024-05-30 17:05:31 发布

qq_478377515

最新推荐文章于 2024-05-30 17:05:31 发布

阅读量1.5k

点赞数

分类专栏： ROS 文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/qq_33031419/article/details/120964228

版权

ROS 专栏收录该内容

11 篇文章 2 订阅

订阅专栏

参考/转载：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/

1 TF

1.1简介

tf的定义不是那么的死板，它可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，所以我们通常称之为"tf tree",tf也可以看成是一个topic:/tf，话题中的message保存的就是tf tree的数据结构格式．维护了整个机器人的甚至是地图的坐标转换关系．tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等．tf含有一部分的接口，就是我们前面章节介绍的roscpp和rospy里关于tf的API．所以可以看成是话题转换的标准，话题，工具，接口．

观察上图，我们可以看到ROS数据结构的一个抽象图，ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的．像上图pr2模型中我们可以看到又很多的frame,错综复杂的铺置在机器人的各个link上，维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通．如下图：

每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系．仔细观察上图，我们发现每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．

1.2 TF消息

每个frame之间都会有broadcaster来发布消息维系坐标转换，这个消息TransformStampde.msg,它就是处理两个frame之间一小段tf的数据格式：

TransformStamped.msg的格式规范如下：

std_mags/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
                flaot64 z
                float64 w

TF tree是由很多的frame之间TF拼接而成，那么TF tree是什么类型呢？如下：

tf/tfMessage.msg
tf2_msgs/TFMessage.msg

（官网建议新工作直接使用tf2，因为它有一个更清洁的界面，和更好的使用体验。如何查看自己使用的TF是哪一个版本，使用命令rostopic info /tf即可）。

tf/tfMessage.msg或tf2_msgs/TFMessage标准格式规范如下:

geometry_msgs/TransformStamped[] transforms
        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
                        flaot64 z
                        float64 w

如上，一个TransformStamped数组就是一个TF tree。

1.3 TF的Python接口

TF库

tf.transformations

基本数学运算函数

函数	注释
euler_matrix(ai,aj,ak,axes='sxyz')	欧拉角到矩阵
eulaer_form_matrix(matrix,axes='sxyz')	矩阵到欧拉角
eular_from_quaternion(quaternion,axes='sxyz')	四元数到欧拉角
quaternion_form_euler(ai,aj,ak,axes='sxyz')	欧拉角到四元数
quaternion_matrix(quaternion)	四元数到矩阵
quaternion_form_matrix(matrix)	矩阵到四元数
......	......

使用该函数库时候，首先import tf，tf.transformations给我们提供了一些基本的数学运算函数如上，使用起来非常方便。在tf_demo中教学包当中，我们列举了一些tf.transformations常见的API和示例代码，具详见下表。

第1部分，定义空间点和空间向量

编号	函数名称	函数功能
1.1	tf.transformations.random_quaternion(rand=None)	返回均匀随机单位四元数
1.2	tf.transformations.random_rotation_matrix(rand=None)	返回均匀随机单位旋转矩阵
1.3	tf.transformations.random_vector(size)	返回均匀随机单位向量
1.4	tf.transformations.translation_matrix(v)	通过向量来求旋转矩阵
1.5	tf.transformations.translation_from_matrix(m)	通过旋转矩阵来求向量

第2部分，定义四元数

编号	函数名称	函数功能
2.1	tf.transformations.quaternion_about_axis(angle axis)	通过旋转轴和旋转角返回四元数
2.2	tf.transformations.quaternion_conjugate(quaternion)	返回四元数的共轭
2.3	tf.transformations.quaternion_from_euler(ai,aj,ak, axes'ryxz')	从欧拉角和旋转轴，求四元数
2.4	tf.transformations.quaternion_from_matrix(matrix)	从旋转矩阵中，返回四元数
2.5	tf.transformations.quaternion_multiply(quaternion1,quaternion2)	两个四元数相乘

第3部分，定义四元数

编号	函数名称	函数功能
3.1	tf.transformations.euler_matrix(ai,aj,ak,axes='xyz')	由欧拉角和旋转轴返回旋转矩阵
3.2	tf.transformations.euler_from_matrix(matrix)	由旋转矩阵和特定的旋转轴返回欧拉角
3.3	tf.transformations.euler_from_quaternion(quaternion)	由四元数和特定的轴得到欧拉角

示例代码：

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  
import rospy  
import math  
import tf  
if __name__ == '__main__':  
    rospy.init_node('py_coordinate_transformation')
#第1部分，定义空间点和空间向量
    print '第1部分，定义空间点和空间向量'
#1.1 返回均匀随机单位四元数
    q=tf.transformations.random_quaternion(rand=None)
    print '定义均匀随机四元数：'
    print q
#1.2 返回均匀随机单位旋转矩阵
    m=tf.transformations.random_rotation_matrix(rand=None)
    print '定义均匀随机单位旋转矩阵：'
    print m
#1.3 返回均匀随机单位向量
    v=tf.transformations.random_vector(3)
    print '定义均匀随机单位向量：'
    print v
#1.4 通过向量来求旋转矩阵
    v_m=tf.transformations.translation_matrix(v)
    print '通过向量来求旋转矩阵：'
    print v_m
#1.5 通过旋转矩阵来求向量
    m_v=tf.transformations.translation_from_matrix(m)
    print '通过旋转矩阵来求向量：'
    print  m_v
#第2部分，定义四元数
    print '第2部分，定义四元数'
#2.1 通过旋转轴和旋转角返回四元数
    axis_q=tf.transformations.quaternion_about_axis(0.123, (1, 0, 0))
    print '通过旋转轴和旋转角返回四元数：'
    print  axis_q
#2.2 返回四元数的共轭
    n_q=tf.transformations.quaternion_conjugate(q)
    print '返回四元数q的共轭：'
    print  n_q
#2.3 从欧拉角和旋转轴，求四元数
    o_q=tf.transformations.quaternion_from_euler(1, 2, 3, 'ryxz')
    print '从欧拉角和旋转轴，求四元数：'
    print  o_q    
#2.4 从旋转矩阵中，返回四元数
    m_q=tf.transformations.quaternion_from_matrix(m)
    print '从旋转矩阵中，返回四元数：'
    print  m_q 
#2.5 两个四元数相乘
    qxq=tf.transformations.quaternion_multiply(q,n_q)
    print '两个四元数相乘'
    print  qxq   
`

TF类

tf.TransformListener类

TransformListener extends TransformerROS, adding a handler for ROS topic /tf_message, so that the messages update the transformer.

方法	作用
canTransform(self,target_frame,source_frame,time)	frame是否相通
waitForTransform(self,target_frame,source_frame,time,timeout)	阻塞直到frame相通
lookup Transform(self,target_frame,source_frame,time)	查看相对的tf，返回（trans，quat）

* canTransform(target_frame, source_frame, time)

Returns True if the Transformer can determine the transform from source_frame to target_frame at the time time. time is a rospy.Time instance.

* canTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame)

It's an extended version of canTransform.

* chain(target_frame, target_time, source_frame, source_time, fixed_frame) -> list

returns the chain of frames connecting source_frame to target_frame.

* frameExists(frame_id) -> Bool

returns True if frame frame_id exists in the Transformer`.

* getFrameStrings -> list

returns all frame names in the Transformer as a list.

* getLatestCommonTime(source_frame, target_frame) -> time

Determines that most recent time for which Transformer can compute the transform between the two given frames, that is, between source_frame and target_frame. Returns a rospy.Time.

* lookupTransform(target_frame, source_frame, time) -> position, quaternion

Returns the transform from source_frame to target_frame at the time time. time is a rospy.Time instance. The transform is returned as position (x, y, z) and an orientation quaternion (x, y, z, w).

* lookupTransformFull(target_frame, target_time, source_frame, source_time, fixed_frame)

It is the full version of lookupTransform.

TransformerROS extends the base class Transformer, adding methods for handling ROS messages.

* asMatrix(target_frame, hdr) -> matrix

Looks up the transform for ROS message header hdr to frame target_frame, and returns the transform as a NumPy 4x4 matrix.

* fromTranslationRotation(translation, rotation) -> matrix

Returns a NumPy 4x4 matrix for a transform.

* transformPoint(target_frame, point_msg) -> point_msg

Transforms a geometry_msgs's PointStamped message, i.e. point_msg, to the frame target_frame, returns the resulting PointStamped.

* transformPose(target_frame, pose_msg) -> pose_msg

Transforms a geometry_msgs's PoseStamped message, i.e. point_msg, to the frame target_frame, returns the resulting PoseStamped.

* transformQuaternion(target_frame, quat_msg) -> quat_msg

Transforms a geometry_msgs's QuaternionStamped message, i.e. quat_msg, to the frame target_frame, returns the resulting QuaternionStamped.

This example uses a TransformListener to find the current base_link position in the map：

import rospy
import tf
from tf import TransformListener
import time

class myNode:

    def __init__(self, *args):
        rospy.init_node('py_tf')
        self.tf = TransformListener()

    def some_method(self):
        time.sleep(1)  # 必须要给一定的反冲时间，否则为all=self.tf.getFrameStrings()空
        All_Frame = TransformListener().getFrameStrings()
        print(All_Frame)
        if self.tf.frameExists("thorvald_001/corner0_1") and self.tf.frameExists("thorvald_001/pipe1_1"):
            print 'ok'
            t = self.tf.getLatestCommonTime("thorvald_001/corner0_1", "thorvald_001/pipe1_1")
            position, quaternion = self.tf.lookupTransform("thorvald_001/corner0_1", "thorvald_001/pipe1_1", t)
            print position, quaternion

if __name__ == '__main__':
    try:
        L = myNode()
        L.some_method()
    except rospy.ROSInterruptException:
        pass

A simple example to transform a pose from one frame to another（根据上面的程序进行补充和修改）：

import rospy
import tf

class myNode:
    def __init__(self, *args):
        self.tf_listener_ = TransformListener()

    def example_function(self):
        if self.tf.frameExists("/base_link") and self.tf.frameExists("/fingertip"):
            t = self.tf_listener_.getLatestCommonTime("/base_link", "/fingertip")
            p1 = geometry_msgs.msg.PoseStamped()
            p1.header.frame_id = "fingertip"
            p1.pose.orientation.w = 1.0    # Neutral orientation
            p_in_base = self.tf_listener_.transformPose("/base_link", p1)
            print "Position of the fingertip in the robot base:"
            print p_in_base

tf.TransformBroadcaster类

类似的，我们介绍的是发布方，tf.TransformBroadcaster类。该类的构造函数也是不需要填值，成员函数有两个如下：

sendTransform(translation,rotation,time,child,parent)#向/tf发布消息
sendTransformMessage(transform)#向/tf发布消息

第一个sendTransform()把transform的平移和旋转填好，打上时间戳，然后表示出从父到子的frame流，然后发向/tf的topic。第二种是发送transform已经封装好的Message给/tf，这两种不同的发送方式，功能是一致的。在tf_demo教学包当中的scripts/py_tf_broadcaster.py和scripts/py_tf_broadcaster02.py给出了示例程序，详见如下。

py_tf_broadcaster.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy  
import math  
import tf    
if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第1种发布方式：sendTransform(translation,rotation,time,child,parent)'
#第一部分，发布sendTransform(translation,rotation,time,child,parent)
    br = tf.TransformBroadcaster()
#输入相对原点的值和欧拉角
    x=1.0 
    y=2.0
    z=3.0  
    roll=0 
    pitch=0
    yaw=1.57 
    rate = rospy.Rate(1)
    while not rospy.is_shutdown(): 
        yaw=yaw+0.1   
        br.sendTransform((x,y,z),  
                     tf.transformations.quaternion_from_euler(roll,pitch,yaw),  
                     rospy.Time.now(),  
                     "base_link",  
                     "link1")  #发布base_link到link1的平移和翻转   
        rate.sleep()

py_tf_broadcaster02.py

#!/usr/bin/env python  
# -*- coding:utf-8 -*-  

import rospy 
import geometry_msgs.msg
import tf2_ros.transform_broadcaster
import math  
import tf   

if __name__ == '__main__':  
    rospy.init_node('py_tf_broadcaster')
    print '讲解tf.transformBroadcaster类'
    print '第2种发布方式：sendTransformMessage(transform)'
#第二部分，发布sendTransformMessage(transform)
    m=tf.TransformBroadcaster()
    t = geometry_msgs.msg.TransformStamped()
    t.header.frame_id = 'base_link'
    t.header.stamp = rospy.Time(0)
    t.child_frame_id = 'link1'
    t.transform.translation.x = 1
    t.transform.translation.y = 2
    t.transform.translation.z = 3
    t.transform.rotation.w=1
    t.transform.rotation.x=0
    t.transform.rotation.y=0
    t.transform.rotation.z=0
#输入相对原点的值和欧拉角
    rate = rospy.Rate(1)
    while not rospy.is_shutdown():
        m.sendTransformMessage(t)
        rate.sleep()

tf包还包括流行的transformations.py模块。TransformROS使用transformations.py(geometry/transformations.py at hydro-devel · ros/geometry · GitHub)在四元数和矩阵之间执行转换。transformations.py对NumPy矩阵进行有用的转换；它可以在欧拉角、四元数和矩阵等变换之间进行转换。

1.4 TF相关工具命令

根据当前的tf树创建一个pdf图：
```
 $ rosrun tf view_frames
```
这个工具首先订阅/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。
查看当前的tf树：
```
 $ rosrun rqt_tf_tree rqt_tf_tree
```
该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。

查看两个frame之间的变换关系：

 $ rosrun tf tf_echo [reference_frame][target_frame]

1.5 Based on the given tf_listener.py and on last week's mover.py, devise your own Python code. Create a publisher that publishes geometry_msgs/PoseStamped messages at the position of the closest laser scan reading and display this pose in Rviz. Some useful pointers:

#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import PoseStamped
from math import cos, sin
from tf import TransformListener

class Mover:
"""
A very simple Roamer implementation for Thorvald.
It simply goes straight until any obstacle is within
3 m distance and then just simply turns left.
A purely reactive approach.
"""

def __init__(self):
"""
On construction of the object, create a Subscriber
to listen to lasr scans and a Publisher to control
the robot
"""
self.publisher = rospy.Publisher(
'/thorvald_001/teleop_joy/cmd_vel',
Twist, queue_size=1)
rospy.Subscriber("/thorvald_001/front_scan", LaserScan, self.callback)
self.pose_pub = rospy.Publisher(
'/nearest_obstacle',
PoseStamped, queue_size=1
)
self.listener = TransformListener()

def callback(self, data):
"""
Callback called any time a new laser scan becomes available
"""

# some logging on debug level, not usually displayed
rospy.logdebug("I heard %s", data.header.seq)

# This find the closest of all laser readings
min_dist = min(data.ranges)

# let's already create an object of type Twist to
# publish it later. All initialised to 0!
t = Twist()

# If anything is closer than 4 metres anywhere in the
# scan, we turn away
if min_dist < 2:
t.angular.z = 1.0
else: # if all obstacles are far away, let's keep
# moving forward at 0.8 m/s
t.linear.x = 0.8
# publish to the topic that makes the robot actually move
self.publisher.publish(t)

# above in min_dist we found the nearest value,
# but to display the position of the nearest value
# we need to find which range index corresponds to that
# min_value.
# find the index of the nearest point
# (trick from https://stackoverflow.com/a/11825864)
# it really is a very Python-ic trick here using a getter
# function on the range. Can also be done with a
# classic for loop
index_min = min(
range(len(data.ranges)),
key=data.ranges.__getitem__)

# convert the obtained index to angle, using the
# reported angle_increment, and adding that to the
# angle_min, i.e. the angle the index 0 corresponds to.
# (is negative, in fact -PI/2).
alpha = data.angle_min + (index_min * data.angle_increment)

# No time for some trigonometry to turn the
# polar coordinates into cartesian coordinates
# inspired by https://stackoverflow.com/a/20926435
# use trigonometry to create the point in laser
# z = 0, in the frame of the laser
laser_point_2d = [
cos(alpha) * min_dist,
sin(alpha) * min_dist,
0.0]

# create an empty PoseStamped to be published later.
pose = PoseStamped()

# keep the frame ID (the entire header here) as read from
# the sensor. This is general ROS practice, as it
# propagates the recording time from the sensor and
# the corrdinate frame of the sensor through to
# other results we may be publishing based on the anaylysis
# of the data of that sensor

pose.header = data.header

# fill in the slots from the points calculated above.
# bit tedious to do it this way, but hey...
pose.pose.position.x = laser_point_2d[0]
pose.pose.position.y = laser_point_2d[1]
pose.pose.position.z = laser_point_2d[2]

# using my trick from https://github.com/marc-hanheide/jupyter-notebooks/blob/master/quaternion.ipynb
# to convert to quaternion, so that the Pose always
# points in the direction of the laser beam.
# Not required if only the positions is
# of relevance
# (then just set pose.pose.orientation.w = 1.0 and leave
# others as 0).
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = sin(alpha / 2)
pose.pose.orientation.w = cos(alpha / 2)

# publish the pose so it can be visualised in rviz
self.pose_pub.publish(pose)#可视化功能，在rviz中界面操作Add -> By topic -> /nearest_obstacle -> OK(https://blog.csdn.net/weixin_43996161/article/details/108493022)

rospy.loginfo(
"The closest point in laser frame coords is at\n%s"
% pose.pose.position
)

# now lets actually transform this pose into a robot
# "base_link" frame.
transformed_pose = self.listener.transformPose("thorvald_001/base_link", pose)
rospy.loginfo(
"The closest point in robot coords is at\n%s"
% transformed_pose.pose.position
)

if __name__ == '__main__':
# as usual, initialise the ROS node with a name
rospy.init_node('mover')
# Create the Mover object
# (which in turns registers the subscriber and make the system go)
Mover()
# Finally, keep going until we are interrupted.
rospy.spin()

***注意这一步将发布的topic可视化：self.pose_pub.publish(pose) #可视化功能，在rviz中界面操作Add -> By topic ->

2 URDF

URDF（Unified Robot Description Format）统一机器人描述格式，URDF使用XML格式描述机器人文件。URDF语法规范，参考链接：urdf/XML - ROS Wiki，URDF组件，是由不同的功能包和组件组成：

2.1制作URDF模型

（1）添加基本模型

（2）添加机器人link之间的相对位置关系

（3）添加模型的尺寸，形状和颜色等

（4）显示URDF模型

2.2 制作xacro模型

什么是Xacro? 我们可以把它理解成为针对URDF的扩展性和配置性而设计的宏语言(macro language)。有了Xacro，我们就可以像编程一样来写URDF文件。XACRO格式提供了一些更高级的方式来组织编辑机器人描述.

2.3 制作gazebo模型

3. TF数学基础

三维空间刚体运动，包括旋转矩阵、欧拉角、四元数：

1）通过旋转矩阵R和平移向量t，我们可以描述从世界坐标系到移动机器人坐标系的坐标变换；

2）旋转本身就是一个很直观的现象。欧拉角可以提供一种非常直观的方式。他利用3个分离的转角，把一次旋转分解成3次绕不同的轴进行旋转。例如先绕x轴旋转，再绕y轴旋转，最后绕z轴旋转，这样就得到一个xyz轴的旋转。在欧拉角中一个常用的是“航偏-俯仰-翻滚”（yaw-pitch-roll）。可以简单记忆rpy-xyz；

3）旋转矩阵用9个量来描述3自由度的旋转，具有冗余性；欧拉角虽然用3个量来描述3自由度的旋转，但是具有万向锁的问题，因此我们选择用四元数，（ROS当中描述转向的都是采用的四元数）。一个四元数拥有一个实部和三个虚部组成：

q=w+xi=yj+zk

从欧拉角到四元数的转换公式，从四元数转化到欧拉角的公式（参考三维空间刚体运动---四元数 · 中国大学MOOC———《机器人操作系统入门》讲义）

qq_478377515

关注

0
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
ros 学习（3）

参考/转载：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/1 TF与URDF1.1简介tf的定义不是那么的死板，它可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，所以我们通常称之为"tf tree",tf也可以看成是一个topic:/tf，话题中的message保存的就是tf tree的数据结构格式．维护了整个机器人的甚至是地图的坐标转换关
复制链接

扫一扫