ultrasonic.py

#!/usr/bin/env python

import rospy
import roslib
import sys
import RPi.GPIO as GPIO
import time
roslib.load_manifest('camera')
from camera.msg import ball
from std_msgs.msg import Int8

distance = 0
MOVE = 1 #moving

TRIG = 23                                  #Associate pin 23 to TRIG
ECHO = 24                                  #Associate pin 24 to ECHO

def findDistance():
    global distance
    GPIO.output(TRIG, False)                 #Set TRIG as LOW
    # print "Waitng For Sensor To Settle"
    time.sleep(2)                            #Delay of 2 seconds

    GPIO.output(TRIG, True)                  #Set TRIG as HIGH
    time.sleep(0.00001)                      #Delay of 0.00001 seconds
    GPIO.output(TRIG, False)                 #Set TRIG as LOW

    while GPIO.input(ECHO)==0:               #Check whether the ECHO is LOW
        pulse_start = time.time()              #Saves the last known time of LOW pulse

    while GPIO.input(ECHO)==1:               #Check whether the ECHO is HIGH
        pulse_end = time.time()                #Saves the last known time of HIGH pulse

    pulse_duration = pulse_end - pulse_start #Get pulse duration to a variable

    distance = pulse_duration * 17150        #Multiply pulse duration by 17150 to get distance
    distance = round(distance, 2)            #Round to two decimal points

    if distance > 1 and distance < 400:      #Check whether the distance is within range
        print "Distance:",distance - 1.2,"cm"  #Print distance with 0.5 cm calibration
    else:
        print "Out Of Range"                   #display out of range
    return

def callback(data):
    global MOVE
    pub = rospy.Publisher('stop', Int8, queue_size = 10)
    # start measuring
    # print "callback!!"
    findDistance()
    # set GPIO pin
    # print("\ntest_callback")
    if distance <= 12:
        pub.publish(not MOVE) # not moving
        # stop robot
        # reset GPIO pins
        return
    pub.publish(MOVE)

def listener():
    rospy.Subscriber("ball_pos", ball, callback)
    # print("\ntest")

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    # Initialize to use BCM numbering.
    GPIO.setmode(GPIO.BCM)

    GPIO.setup(TRIG,GPIO.OUT)                  #Set pin as GPIO out
    GPIO.setup(ECHO,GPIO.IN)                   #Set pin as GPIO in

    rospy.init_node('ultrasonic', anonymous=True)
    print ("working")
    listener()