写在前面
上一节我们简单讨论了在生成点云的过程中的矩阵变换。如果大家看本章有点懵逼的话，不妨先看一下我上节写的内容。 我们本节将基于激光雷达与矩阵变换关系，将激光雷达的LaserScan数据转换为三维空间下的PointCloud点云数据。 本节涉及到知识点主要为：
- ROS中的TF空间变换的读取和发布
- 激光雷达数据LaserScan与基础点云（PointCloud）的消息类型
激光雷达与地图原点之间的坐标变换
基于gmapping获取2D平面下的变换关系
我们在第五节说到了机器人基于gmapping在二维空间下的2D导航。在这个导航中，我们使用了以下节点，发布了一条从/map到/laser之间的变换链路： 
我们也可以在实际地图看到坐标变换的关系，例如gmapping发布的由map到base_link的变换，由动态TF发布节点发布的base_link到laser的变换： 
如果正确的配置了gmapping实现二维定位，我们就可以通过ROS中的TF变换系统，直接获取到laser到map之间的6自由度坐标变换；代码如下：
self.listener=tf.TransformListener()
def Laser2GlobalTrans(self,para='local'):
'''
从激光雷达坐标系到地图坐标系的变换矩阵构建
'''
# 获取激光雷达到Global坐标系的变换
if para=='global':
for i in range(10):
try:
#rospy.sleep(0.01)
self.listener.waitForTransform('/map', '/laser', rospy.Time(), rospy.Duration(0.1))
(trans,rot)=self.listener.lookupTransform('/map','/laser', rospy.Time(0.0))
break
except:
continue
else:
for i in range(10):
try:
#rospy.sleep(0.01)
self.listener.waitForTransform('/base_link', '/laser', rospy.Time(), rospy.Duration(0.1))
(trans,rot)=self.listener.lookupTransform('/base_link','/laser', rospy.Time(0.0))
break
except:
continue
roll,pitch,yaw=tf.transformations.euler_from_quaternion(rot)
值得注意点是，代码中的Listener一定要设置为静态，全局或只申明一次的变量，如果每一次运行都声明一个listener的话，系统运行会越来越卡。 我们可以看到，其中的关键代码为：
self.listener.waitForTransform('/base_link', '/laser', rospy.Time(), rospy.Duration(0.1))
(trans,rot)=self.listener.lookupTransform('/base_link','/laser', rospy.Time(0.0))
这两行代码的意思为获取由laser到baseLink的空间变换，其中trans为一个三元素数据，分别代表x，y，z轴的空间偏移。而rot是一个四元数数据。我们在这里不去详细分析它，只用一个函数把它转换为我们上一节所说的roll，pitch和yaw。
roll,pitch,yaw=tf.transformations.euler_from_quaternion(rot)在获取了这些参数后，我们就能够得到上一节所说的空间变换矩阵了。
基于yaw，pitch以及舵机的臂长度构建变换矩阵
首先来看一下我们云台的形态： 
一个2自由度的变换，我们可以一个自由度一个自由度的拆解，首先假设只有yaw，没有pitch（也就是只有z轴旋转，没有俯仰角），那么我们假设旋转的中心点为O，yaw旋转中心点距离绿色舵机中心的距离为l，那么如下图，我们可以得出pitch层面的矩阵变换（以旋转点为（0，0）点）为： 
我们假设这个矩阵为矩阵A，那么yaw变换后，任意一个激光回波点相对云台正中心的坐标与相对于激光雷达中心的坐标为：
pitch坐标变换与yaw同理，我们假设纵向机械臂长度为L（大写L），pitch的变换矩阵为：
融合后，矩阵为：
当然，实际操作的时候矩阵行列可能不相等，我们把矩阵整体写成4X4就可以了，其中height为激光雷达的原始高度（就是pitch，yaw都为0时的高度）。
ROS发布base到laser的TF变换
在ROS中，如果想获取A到B之间的空间变换，那么我们必须要构建一条完整的空间变换链路。 举一个例子，我们在前面提到了由map到laser之间的变换，并且展示了空间变换的一张图片。我们可以看出，在map到laser之间，有一条完整的“链条”： map—>odom—>base_footprint—>base_link—>laser
我们已经发布了从base_link—>laser的变换；再结合其他的变换体系，我们就够构建了从map到laser的整条链路。这个链条也就是构建由map到laser变换的基础，如果base_link—>laser之间的坐标变换缺失，map到laser之间的链路就断开，整个系统也就跑不起来了。
在我们的设计中，base_link是机器人的底座；而laser代表了机器人激光雷达的坐标。我们基于云台的2个舵机的转角时刻发布这个坐标变换。 我们先来看一下完整的发布坐标变换的函数：
def BroadCast_Head_TF(pitch=0.0,yaw=1.57,base_coord=[0.0,0.0,0.0],bar_l=0.05):
'''
构建从雷达平面到三维空间的变换矩阵
'''
transMatrix1=[[np.cos(yaw),0,0,-bar_l*np.sin(yaw)+base_coord[0]],\
[0,1,0,base_coord[1]],\
[np.sin(yaw),0,0,bar_l*np.cos(yaw)+base_coord[2]],\
[0,0,0,0]]
br = tf.TransformBroadcaster(queue_size=100)
br.sendTransform((base_coord[0]-bar_l*np.sin(yaw),base_coord[1], base_coord[2]+bar_l*np.cos(yaw)),
tf.transformations.quaternion_from_euler(0, yaw, pitch-np.pi),
rospy.Time.now(),
'/laser',
"/base_link")
关键的函数如下：
br = tf.TransformBroadcaster(queue_size=100)
br.sendTransform((x,y,z),
tf.transformations.quaternion_from_euler(roll,pitch,yaw),
rospy.Time.now(),
'/laser',
"/base_link")我们首先构建了一个tf.TransformBroadcaster,值得注意的是：这个Broadcaster要只创建一次，然后重复使用，不要反复创建，否则TF系统会卡死。
接下来，我们发布坐标变换： br.sendTransform([x,y,z Tuple]，[旋转四元数],发布TF时间，子坐标系，母坐标系)
由LaserScan到PCL数据
雷达数据到PointCloud转换的流程
我们之前也说到了LaserScan的消息结构。如果我们获取了激光雷达的数据，也获取了由激光雷达到地图原点/map之间的变换矩阵，那么我们就可以将一个激光雷达点的坐标映映射到三维空间坐标。 这个变换的整体逻辑如下： 
构建并发布PointCloud数据
我们首先来看一下PointCloud的结构体：
Header header
geometry_msgs/Point32[] points #Array of 3d points
ChannelFloat32[] channels
这个消息支持将三维空间中的点的数组以及任何保存在一个信道中的相关数据。 例如，一条带有”intensity”信道的 PointCloud 可以保持点云数据中每一个点的强度。 这个消息中的points数据形成的数据组，就是整个空间全部的点云。 我们可以通过如下的代码来实现点云的发布，它是基于一个激光雷达的回调函数的：
def Scan(self):
rospy.wait_for_service('/head_rotate')
rospy.loginfo('start scanning...')
#初始化local点云
self.pcl_Local.points=[]
self.pcl_Local.channels=[]
for line in range(self.scanTrigger.lines):
self.draw_Percentage.publish(Int32(line))
#控制云台旋转到指定角度（基于yaw做z轴扫描）
self.head_Ctrl(dof2_srvRequest(0.0,self.scanTrigger.init_angle+line*self.scanTrigger.angle_increment))
self.has_Scan=False
while not self.has_Scan:
rospy.sleep(0.01)
'''
激光雷达数据转换为直角坐标数据
'''
#获取雷达坐标系到空间坐标系的三维变换
'''
matP: y轴旋转变换
matY：z轴旋转变换
matR：绕X轴旋转变换（因为机器人原则上不会绕X
轴旋转，所以没这个参数）
matShift：空间三坐标偏移
'''
matP,matY,matShift=self.Laser2GlobalTrans('local')
rospy.sleep(0.01)
matGP,matGY,matGShift=self.Laser2GlobalTrans('global')
#获取激光雷达点数
ptns=len(self.laser_Cache.ranges)
for i in range(ptns):
#计算极坐标的两个参数
p=self.laser_Cache.ranges[i]
w=self.laser_Cache.angle_min+self.laser_Cache.angle_increment*i
#进行简单滤波
if p>self.scanTrigger.max_ranges or p<self.scanTrigger.min_ranges: continue
x=p*np.cos(w)
y=p*np.sin(w)
#回波点整合为平面直角坐标系坐标(列矩阵)
coord_Laser=np.matrix([[x],[y],[0]])
#对单个回波点进行坐标变幻
coord_XYZ=(matY*(matP*coord_Laser)+matShift).tolist()
if coord_XYZ[2][0]>0.14:
self.pcl_Local.points.append(Point32(coord_XYZ[0][0],coord_XYZ[1][0],coord_XYZ[2][0]))
if coord_GXYZ[2][0]>0.14:
coord_GXYZ=(matGY*(matGP*coord_Laser)+matGShift).tolist()
self.pcl_Global.points.append(Point32(coord_GXYZ[0][0],coord_GXYZ[1][0],coord_GXYZ[2][0]))
self.pcl_Local.header.stamp=rospy.Time.now()
self.pcl_Local.header.seq+=1
#初始化Global 点云
self.pcl_Global.header.stamp=rospy.Time.now()
self.pcl_Global.header.seq+=1
self.local_Pub.publish(self.pcl_Local)
self.global_Pub.publish(self.pcl_Global)
self.head_Ctrl(dof2_srvRequest(0.0,0.0))
self.draw_Percentage.publish(Int32(0))
本章小节
如果大家看本章有点懵逼的话，不妨先看一下我上节写的内容。 本章三个重要知识点，也是由LaserScan到PointCloud变换的流程包含三个技术点：
- 激光雷达数据的形态，PointCloud数据的形态和收发
- TF坐标系系的读取和发布
- 矩阵变换
下一小节，我们将基于三维点云的发布，实现更高层级的二次开发，例如在Windows和Linux机器上进行协同配置，实现一台“样机化”的3D空间测绘机器人。 拭目以待~
联系作者
