Skip to content

Instantly share code, notes, and snippets.

@okalachev
Last active January 30, 2022 23:06
  • Star 0 You must be signed in to star a gist
  • Fork 1 You must be signed in to fork a gist
Star You must be signed in to star a gist
Save okalachev/feb2d7235f5c9636802c3cda43add253 to your computer and use it in GitHub Desktop.
ROS node that filters and publishes data from HC-SR04 rangefinder
# -*- 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 rangefinder pin
ECHO = 24 # Echo rangefinder pin
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 - number of samples for median filter
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