python写的ROS激光雷达扇形滤波

python写的ROS激光雷达扇形滤波

项目中想要实现针对机器人正前方N°的区域有障碍时，对机器人进行速度限制，于是自己做了一个激光雷达的扇形滤波。

思路就是获取/scan话题，将话题数据中的最大和最小角度设置为自己想要的扇区，需要注意的是，LaserScan一般默认的坐标系与机器人方向相反，A1雷达的背面是laser坐标系的正方向——0°角位置

在使用python时，发现LaserScan的ranges使用元组来储存，无法直接修改或删除，但是可以使用赋值的方式修改，修改数据时还需要注意角度和距离数组的对应关系。

程序可以通过参数设置扇区的大小。

附上代码

class Fliter:
    def __init__(self):
        rospy.init_node('LaserScanFliter',anonymous=True)

        self.receive_scan = rospy.Subscriber('/scan', LaserScan , self.callback)
        self.publish_scan = rospy.Publisher('/scan_range_filter',LaserScan,queue_size=1)
        self.scan_degree = rospy.get_param('~degree',6)



    def callback(self,data):
        new_scan = data
        new_scan.angle_min=3.1415926-3.1415926/self.scan_degree*2
        #print(new_scan)
        ranges=()
        range_size = len(data.ranges)
        range_size_pair = int(range_size/self.scan_degree)
        print(range_size_pair)
        for i in range(range_size_pair*(self.scan_degree-1),range_size-1):
            ranges += (data.ranges[i],)
        for i in range(0,range_size_pair):
            ranges += (data.ranges[i],)


        new_scan.ranges=ranges
        new_scan.angle_max=new_scan.angle_min+new_scan.angle_increment*(len(ranges)-1)
        self.publish_scan.publish(new_scan)