1.雷達(dá)數(shù)據(jù)屏蔽需求

使用激光雷達(dá)建圖和導(dǎo)航,實(shí)際應(yīng)用中,不可能保證雷達(dá)在最上層(即車體不會(huì)遮擋雷達(dá)),一般應(yīng)用為雷達(dá)放在下面,對(duì)于360°的雷達(dá),車體就會(huì)遮擋從而導(dǎo)致車體被人作為障礙物,那如何解決這個(gè)問題

2.簡(jiǎn)單實(shí)現(xiàn)

我們只需要訂閱激光雷達(dá)的數(shù)據(jù),把相應(yīng)角度的數(shù)據(jù)屏蔽即可,然后在重新發(fā)布新的數(shù)據(jù)

#!/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
        newdata.ranges = list(data.ranges)
        newdata.intensities = list(data.intensities)

        for x in range(120,130):
            newdata.ranges[x]=0
            newdata.intensities[x]=0

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

if __name__ == '__main__':
    rospy.init_node('LidarFilter', anonymous=False)
    lidar = DoFilter()

    rospy.spin()

3.laser_filters包

3.1 LaserScanBoxFilter

先貼地址 laser_filters - ROS Wiki,改包提供了強(qiáng)大的功能,這里我們使用一個(gè)簡(jiǎn)單的例子看下如何屏蔽

我們先寫個(gè)模擬的激光雷達(dá),假設(shè)1m范圍內(nèi)為障礙物

scan.png

代碼貼上如下,不在贅述

#!/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 ScanSimulator:
    def __init__(self):

        self.pub = rospy.Publisher("scan", LaserScan, queue_size=10)
        self.newdata = LaserScan()
        self.newdata.ranges = [0]*360
        self.newdata.intensities = [0]*360
        self.newdata.header.frame_id="laser_link"

        self.newdata.angle_min=-1.57
        self.newdata.angle_max=1.57
        self.newdata.angle_increment=3.14*2 / 360
        self.newdata.time_increment=(1 / 100) / (360);
        self.newdata.range_min = 0.0;
        self.newdata.range_max = 100.0;

    def pub(self):
        self.newdata.header.stamp=rospy.Time.now()
        for x in range(0,360):
            self.newdata.ranges[x]=1
            self.newdata.intensities[x]=1

        self.pub.publish(self.newdata)

if __name__ == "__main__":
    rospy.init_node('scan_simulator',anonymous=True)

    r = rospy.Rate(100)
    lidar = ScanSimulator()

    while not rospy.is_shutdown():
        lidar.pub()
        r.sleep()

下面看下如何使用該包

box_filter_example.launch

<launch>
<node pkg="laser_filters" type="scan_to_scan_filter_chain" output="screen" name="laser_filter">
      <rosparam command="load" file="$(find pibot_bringup)/params/box_filter.yaml" />
</node>
</launch>

box_filter.yaml

scan_filter_chain:
- name: box_filter
  type: laser_filters/LaserScanBoxFilter
  params:
    box_frame: laser_link
    min_x: -1.0
    max_x: 1.0
    min_y: -0.5
    max_y: 0.5
    min_z: -0.1
    max_z: 0.1

使用該配置,我們屏蔽min_至max_ 形成的box里面的數(shù)據(jù), 我們通過啟動(dòng)該launch后查看數(shù)據(jù)的filter_scan見下圖

filter_scan.png

box_filter.yaml

scan_filter_chain:
- name: box_filter
  type: laser_filters/LaserScanBoxFilter
  params:
    box_frame: laser_link
    min_x: -1.0
    max_x: 1.0
    min_y: -0.75
    max_y: 0.5
    min_z: -0.1
    max_z: 0.1

使用該配置效果如下圖

filter_scan.png

3.2 scan_to_scan_filter_chain

scan_to_scan_filter_chain.png

可以看到該node為一個(gè)鏈,通過插件可以配置多層的過濾,可以配置下列濾波器

• LaserScanBoxFilter 無視一個(gè)區(qū)塊內(nèi)的數(shù)據(jù)（常用于無視機(jī)器人本體對(duì)激光雷達(dá)數(shù)據(jù)的干擾）
• ScanShadowsFilter 針對(duì)物體邊沿的掃描和識(shí)別
• InterpolationFilter 插值濾波
• LaserScanIntensityFilter 設(shè)定強(qiáng)度閾值,超出則設(shè)置為nan
• LaserScanRangeFilter 設(shè)定距離閾值,超出則設(shè)置為nan
• LaserScanAngularBoundsFilter 將設(shè)定的角度外的掃描數(shù)據(jù)刪除
• LaserScanAngularBoundsFilterInPlace 不會(huì)刪除目標(biāo)角度扇區(qū)外的數(shù)據(jù)，但會(huì)把對(duì)應(yīng)掃描的距離值設(shè)為最大距離閾值+1
• LaserArrayFilter 使用中值過濾器等對(duì)距離和強(qiáng)度進(jìn)行過濾