ros-robotics 原文存档
树莓派ubuntu使用gpio连超声波发送到ros topic
超声波使用的是HC-SR04
1，安装树莓派gpio库，https://pypi.org/project/RPi.GPIO/
pip install RPi.GPIO2，然后如下代码是无ros的，直接运行的代码：
`#!/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)`3，程序正常运行，如果不正常，比如说
This Channel is already in use, continouing anyway这是因为需要程序末尾加如下语句，释放端口
GPIO.cleanup()4，运行程序，如果说没有权限操作gpio，那么是因为想要操作gpio，普通用户权限不足。
我们可以用下面的脚本，来开启普通用户能使用gpio的权限
`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()`5，使用把超声波作为ros的包，来发送ros topic距离信息
`#!/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()`下面是CMakeLists.txt文件
`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} )`下面是package.xml文件
`<?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>`6，话题接收者和处理