ROS之激光雷达

激光雷达已经成为机器人标配了，ROS作为通用机器人系统也支持激光雷达数据。激光雷达产生的数据类型是sensor_msgs/LaserScan或sensor_msgs/PointCloud,或者是新的sensor_msgs/PointCloud2。对于我手头的这台Turtlebot3而言，LDS设备的数据类型是sensor_msgs/LaserScan。ROS中的传感器数据非常多，但是这些数据流常常需要同步，所以ROS定义了一个通用的数据头，各种数据都会带上这个header。

ROS header的数据结构

类型Header包括多个字段。字段seq对应一个标识符，随着消息被发布，它会自动增加。字段stamp存储与数据相关联的时间信息。以激光扫描为例，stamp可能对应每次扫描开始的时间。字段frame_id存储与数据相关联的tf帧信息。以激光扫描为例，它将是激光数据所在帧。

#Standard metadata for higher-level flow data types
#sequence ID: consecutively increasing ID 
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id

LaserScan消息

如前文所述，ROS提供了一个特殊的消息类型LaserScan来存储激光信息。LaserScan消息是ROS激光扫描数据的软件层表示。LaserScan消息的规范定义如下：

#
# 测量的激光扫描角度，逆时针为正
# 设备坐标帧的0度面向前（沿着X轴方向）
#

Header header
float32 angle_min        # scan的开始角度 [弧度]
float32 angle_max        # scan的结束角度 [弧度]
float32 angle_increment  # 测量的角度间的距离 [弧度]
float32 time_increment   # 测量间的时间 [秒]
float32 scan_time        # 扫描间的时间 [秒]
float32 range_min        # 最小的测量距离 [米]
float32 range_max        # 最大的测量距离 [米]
float32[] ranges         # 测量的距离数据 [米] (注意: 值 < range_min 或 > range_max 应当被丢弃)
float32[] intensities    # 强度数据 [device-specific units]

在上面的数据结构中，range_min和range_max表示激光雷达s的测量范围，因为激光雷达在太近和太远的地方都不准确。ranges则是一个数组，里面存放的就是距离数值。Turtlebot3自带的激光传感器是一个平面的距离传感器，它的外形大概长这样：

LDS-01的相关测量参数如下：

如表中所示，这个LDS的角度分辨率是1度，所以对应的ranges数组的长度就是360。每一次扫描的结果在平面上展示出来，其实就是一圈点，如下图所示。每一个红点就对应ranges数组的一个距离，所以这是一个极坐标的图。

LaserScan数据的使用

既然LaserScan已经可以实现距离测量，那么实现障碍物检测就不算一件复杂的事情了。下面是一个障碍物检测的例子，实现的功能也很简单，就是一直行，直到遇到障碍物然后停下来。程序的逻辑也很简单的，不断获取“/scan”这个topic的数据，然后筛选出有效数据和机器人行进前方的距离；然后判断最近距离的大小，如果最近距离大于20厘米则继续前行，如果最近距离小于20厘米则立即停下来避免碰撞。

当然这只是激光传感器的最简单使用了，SLAM和导航才是LDS最重要的应用场景。

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

class Obstacle():
    def __init__(self):
        self.LIDAR_ERR = 0.05
        self._cmd_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
        self.obstacle()
        
    def get_scan(self):
        msg = rospy.wait_for_message("scan", LaserScan)
        self.scan_filter = []
        for i in range(360):
            if i <= 15 or i > 335:
                if msg.ranges[i] >= self.LIDAR_ERR:
                    self.scan_filter.append(msg.ranges[i])
                    
    def obstacle(self):
        self.twist = Twist()
        while not rospy.is_shutdown():
            self.get_scan()

            if min(self.scan_filter) < 0.2:
                self.twist.linear.x = 0.0
                self.twist.angular.z = 0.0
                self._cmd_pub.publish(self.twist)
                rospy.loginfo('Stop!')

            else:
                self.twist.linear.x = 0.05
                self.twist.angular.z = 0.0
                rospy.loginfo('distance of the obstacle : %f', min(self.scan_filter))

            self._cmd_pub.publish(self.twist)

def main():
    rospy.init_node('turtlebot3_obstacle')
    try:
        obstacle = Obstacle()
    except rospy.ROSInterruptException:
        pass

if __name__ == '__main__':
    main()