# -*- 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) |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment