Instantly share code, notes, and snippets.

Embed
What would you like to do?
# -*- 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