sonar.py

# -*- coding: utf-8 -*-

import time
import threading
import pigpio
import collections
import numpy
import rospy
from sensor_msgs.msg import Range

rospy.init_node('sonar')
dist_pub = rospy.Publisher('~distance', Range, queue_size=1)
filt_dist_pub = rospy.Publisher('~filtered_distance', Range, queue_size=1)

TRIG = 23  # пин, к которому подключен контакт Trig дальномера
ECHO = 24  # пин, к которому подключен контакт Echo дальномера

pi = pigpio.pi()
done = threading.Event()

def rise(gpio, level, tick):
    global high
    high = tick

def fall(gpio, level, tick):
    global low
    low = tick - high
    done.set()

def read_distance():
    done.clear()
    pi.gpio_trigger(TRIG, 50, 1)
    done.wait(timeout=5)
    return low / 58.0 / 100.0

pi.set_mode(TRIG, pigpio.OUTPUT)
pi.set_mode(ECHO, pigpio.INPUT)
pi.callback(ECHO, pigpio.RISING_EDGE, rise)
pi.callback(ECHO, pigpio.FALLING_EDGE, fall)

history = collections.deque(maxlen=10)  # 10 - количество сэмплов для усреднения
r = Range()

while not rospy.is_shutdown():
    distance = read_distance()
    history.append(distance)
    filtered_distance = numpy.median(history)
    r.header.stamp = rospy.get_rostime()
    r.range = distance
    dist_pub.publish(r)
    r.range = filtered_distance
    filt_dist_pub.publish(r)