#!/usr/bin/python
import time
import RPi.GPIO as GPIO

trigger_pin = 37
echo_pin = 35

GPIO.setmode(GPIO.BOARD)
GPIO.setup(trigger_pin,GPIO.OUT)
GPIO.setup(echo_pin,GPIO.IN)

def send_trigger_pulse():
    GPIO.output(trigger_pin,True)
    time.sleep(0.0001)
    GPIO.output(trigger_pin,False)

def wait_for_echo(value,timeout):
    count = timeout
    while GPIO.input(echo_pin) != value and count>0:
        count = count-1

def get_distance():
    send_trigger_pulse()
    wait_for_echo(True,10000)
    start = time.time()
    wait_for_echo(False,10000)
    finish = time.time()
    pulse_len = finish-start
    distance_cm = pulse_len/0.000058
    return distance_cm

while True:
    print("cm = %f"%get_distance())
    time.sleep(1)

import grp
import subprocess

def ensure_gpiogroup():
    try:
        grp.getgrnam('gpio')
    except KeyError:
        print('GPIO group does not exist - creating...')
        subprocess.call(['groupadd', '-f', '-r', 'gpio'])
        subprocess.call(['adduser', '你的树莓派用户名', 'gpio'])
        # in future, also for groups:
        #   spi
        #   i2c
        add_udev_rules()

def add_udev_rules():
    with open('/etc/udev/rules.d/99-gpio.rules','w') as f:
        f.write("""SUBSYSTEM=="bcm2835-gpiomem", KERNEL=="gpiomem", GROUP="gpio", MODE="0660"
SUBSYSTEM=="gpio", KERNEL=="gpiochip*", ACTION=="add", PROGRAM="/bin/sh -c 'chown root:gpio /sys/class/gpio/export /sys/class/gpio/unexport ; chmod 220 /sys/class/gpio/export /sys/class/gpio/unexport'"
SUBSYSTEM=="gpio", KERNEL=="gpio*", ACTION=="add", PROGRAM="/bin/sh -c 'chown root:gpio /sys%p/active_low /sys%p/direction /sys%p/edge /sys%p/value ; chmod 660 /sys%p/active_low /sys%p/direction /sys%p/edge /sys%p/value'"
""")

if __name__ == '__main__':
    ensure_gpiogroup()

#!/usr/bin/env python

import RPi.GPIO as gpio
import time
import sys
import signal
import rospy
from std_msgs.msg import Float32

def signal_handler(signal, frame): # ctrl + c -> exit program
        print('You pressed Ctrl+C!')
        sys.exit(0)
signal.signal(signal.SIGINT, signal_handler)


class sonar():
    def __init__(self):
        rospy.init_node('sonar')
        self.distance_publisher = rospy.Publisher('/sonar_dist',Float32, queue_size=1)
        self.r = rospy.Rate(1)
    def dist_sendor(self,dist):
        data = Float32()
        data.data=dist
        self.distance_publisher.publish(data)
        
        
gpio.setmode(gpio.BOARD)
trig = 37 # 7th
echo = 35 # 6th

gpio.setup(trig, gpio.OUT)
gpio.setup(echo, gpio.IN)
gpio.setwarnings(False)

sensor=sonar()
time.sleep(0.5)
print ('-----------------------------------------------------------------sonar start')
try :
    while True :
        gpio.output(trig, False)
        time.sleep(0.1)
        gpio.output(trig, True)
        time.sleep(0.00001)
        gpio.output(trig, False)
        while gpio.input(echo) == 0 :
            pulse_start = time.time()
        while gpio.input(echo) == 1 :
            pulse_end = time.time()
        pulse_duration = pulse_end - pulse_start
        distance = pulse_duration * 17000
        if pulse_duration >=0.11746:
            print('time out')
            continue
        elif distance > 300 or distance==0:
            print('out of range')
            continue
        distance = round(distance, 3)
        print ('Distance : %f cm'%distance)
        sensor.dist_sendor(distance)
        
        sensor.r.sleep()
        
except (KeyboardInterrupt, SystemExit):
    gpio.cleanup()
    sys.exit(0)
except:
    gpio.cleanup()

cmake_minimum_required(VERSION 2.8.3)
project(sonar)

find_package(catkin REQUIRED COMPONENTS
  roslib
  rospy
  std_msgs
)

catkin_package(

)

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

<?xml version="1.0"?>
<package format="2">
  <name>sonar</name>
  <version>0.0.0</version>
  <description>The sonar package</description>

  <maintainer 
	email="blog.openpilot.cc">Zou Zhe
  </maintainer>

  <license>TODO</license>

  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>roslib</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_export_depend>roslib</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <exec_depend>roslib</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>std_msgs</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>