ROS激光雷达数据过滤

ROS激光雷达数据过滤

尝试使用一下Markdown编辑器，html编辑器用到爆炸

目录

ROS与激光雷达

激光雷达是避障与SLAM常用的装备，ROS中也对激光雷达做出了相应的支持，各个厂家也有不少提供了自己的ROS库供开发者使用，使得无论是初学者还是大神们都能轻松愉快的使用激光雷达为自己的机器人导航。
ROS中的sensor_msgs/LaserScan提供了最基础的针对一线激光雷达消息格式：

# 一线激光雷达的一次扫描

Header header            # timestamp是扫描中第一束激光的获取时间

                         # 角度环绕Z轴（Z轴为激光雷达正上方），逆时钟方向增加，0度角指向X轴
float32 angle_min        # 扫描的起始角度 [rad]
float32 angle_max        # 扫描的结束角度 [rad]
float32 angle_increment  # 每次测量间的角度差 [rad]

float32 time_increment   # 每次测量间的时间差 [seconds] 
                         # 这个数值会用来生成点云（而不是scan_time）
float32 scan_time        # 每次扫描的时间差 [seconds] 
                         # 这个时间差是每次激光发射的时间差，上面那个是每次接收（测量）的时间差

float32 range_min        # 距离最小值 [m]
float32 range_max        # 距离最大值 [m]

float32[] ranges         # 距离数据 [m]
float32[] intensities    # 强度数据 [单位视设备标定] 
                         #如果设备不产生强度数据，则该数据为空

通过这个消息提供的信息，就可以开始进行数据的

获取激光雷达数据

首先要根据使用的激光雷达设备，启动相应的驱动节点来进行扫描和测量。数据一般来说会发布到scan这个主题里，只要订阅一下就可以获取到激光雷达的数据了

#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan

def callback(scan):
    #LaserScan消息的格式
    #std_msgs/Header header
    #float32 angle_min
    #float32 angle_max
    #float32 angle_increment
    #float32 time_increment
    #float32 scan_time
    #float32 range_min
    #float32 range_max
    #float32[] ranges
    #float32[] intensities

    rospy.loginfo(scan.header)

def listener():
    rospy.init_node('laser_listener', anonymous=False)
    rospy.Subscriber('scan', LaserScan, callback)
    rospy.spin()

if __name__ == '__main__':
    listener()

过滤激光雷达数据并重新发布

通过对激光雷达节点传过来的数据包进行过滤，从而获取某一扇区的雷达数据

#!/usr/bin/env python

from __future__ import print_function
import sys
import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String

class DoFilter:
    def __init__(self):

        self.sub = rospy.Subscriber("scan", LaserScan, self.callback)
        self.pub = rospy.Publisher("filteredscan", LaserScan, queue_size=10)

    def callback(self, data):

        newdata = data
        #从消息中读取的距离和强度数据是tuple，需要转成list以便操作
        newdata.ranges = list(data.ranges)
        newdata.intensities = list(data.intensities)

        #通过清除不需要的扇区的数据来保留有效的数据
        for x in range(120,240):
            newdata.ranges[x]=0
            newdata.intensities[x]=0

        #前方180°的扇区
        #for x in range(90,270):
        #    newdata.ranges[x]=0
        #    newdata.intensities[x]=0

        #正前方60°的扇区
        #for x in range(30,330):
        #    newdata.ranges[x]=0
        #    newdata.intensities[x]=0

        self.pub.publish(newdata)
        rospy.loginfo(data)


if __name__ == '__main__':

    # Initialize
    rospy.init_node('LidarFilter', anonymous=False)
    lidar = DoFilter()

    rospy.spin()

通过laser_filters进行过滤

除了上面比较关爱我这水平的战5渣的方法外，还有比较高端的laser_filters进行激光雷达数据过滤的方法，详情请参见：

laser_filters的wiki页
龟机3跟人应用的激光雷达过滤器

在yaml文件里面定义了很多laser_filters消息的参数后，通过启动laser_filters节点完成激光雷达消息的过滤。其中scan_to_scan_filter_chain提供了从初始扫描到过滤后扫描的流程：

而scan_to_cloud_filter_chain提供了从scan消息生成过滤后的点云的流程：

这两类的示例yaml和launch都可以在上面提供的wiki页找到，很有参考价值的（基本上拷下来调调参数就能用了）
设置参数放在yaml里，消息转换和参数加载放在launch里
除了这些之外，还有必要说下一部分比较常用的参数类型：

InterpolationFilter

将扫描中出现的异常数据通过使用两侧的正常数据进行补齐，效果如下：

.....`..... => InterpolationFilter => ...........

参数设置示例：

scan_filter_chain:
- name: interpolation
  type: InterpolationFilter

LaserScanIntensityFilter

强度值过滤器，将超出范围的扫描结果中的距离结果设为最大距离+1，从而将结果标记为异常。_{这思路有点迷(´⊙ω⊙`)}
参数有三个：

lower_threshold (double)  最小阈值
upper_threshold (double)  最大阈值
disp_histogram (int)      是否输出柱状图(°Д°)居然有这功能，牛逼

参数设置示例：

scan_filter_chain:
- name: intensity
  type: LaserScanIntensityFilter
  params:
    lower_threshold: 8000
    upper_threshold: 100000
    disp_histogram: 0

LaserScanRangeFilter

距离值过滤器，会把超过设定范围阈值的数据改为NaN或根据设定进行更改。另外可以读取scan消息中的范围阈值信息。

lower_threshold (double) 最小阈值
upper_threshold (double) 最大阈值
use_message_range_limits (bool) 是否使用scan消息中的阈值信息，默认为false
lower_replacement_value (double) 将小于最小阈值的数据改为该值，默认NaN
upper_replacement_value (double) 将大于最大阈值的数据改为该值，默认NaN

参数设置示例：

scan_filter_chain:
- name: range
  type: LaserScanRangeFilter
  params:
    use_message_range_limits: false
    lower_threshold: 0.3
    upper_threshold: .inf
    lower_replacement_value: -.inf
    upper_replacement_value: .inf

LaserScanAngularBoundsFilter

将设定的角度外的扫描数据删除

lower_angle (double) 最小角度（rad）
upper_angle (double) 最大角度（rad）

参数设置示例：

scan_filter_chain:
- name: angle
  type: LaserScanAngularBoundsFilter
  params:
    lower_angle: -1.57
    upper_angle: 1.57

其他参数

其他参数还包括

LaserScanBoxFilter 无视一个区块内的数据（常用于无视机器人本体对激光雷达数据的干扰）
LaserScanAngularBoundsFilterInPlace 不会删除目标角度扇区外的数据，但会把对应扫描的距离值设为最大距离阈值+1
ScanShadowsFilter 针对物体边沿的扫描和识别
LaserArrayFilter 使用数学过滤器对距离和强度进行过滤

结语

Markdown编辑器好好用ヾ(๑╹◡╹)ﾉ以后就用这个了
激光雷达现在手头只有一线的，据说之后会拿到16线的，关于点云的部分就到时候再说吧
毕竟想要彻底的理解和熟练掌握一个科技，还是要实际的动手去做一下比较好
这篇真的有够长的。。。写的我都饿了ψ(｀∇´)ψ食堂走起！！
然而为了减肥，我决定从今天起每天中午只吃水煮青菜（麻辣烫+青菜-肉-面食=水煮青菜）