Der Vollständigkeit erst mal eine Erklärung noch ohne ROS: Eine LED mit dem Raspberry Pi mit Python und RPi.GPIO Library ist denkbar einfach.
Blinky – noch ohne ROS
Das nachfolgende „blinky“-Beispiel mag sehr lang aussehen, das liegt aber daran, dass gleich ein Signal-Handler mit im Code ist, der beim Drücken von CTRL-C auf der Tastatur noch Aktionen ausführt, bevor das Programm beendet wird.
#!/usr/bin/python
# coding=utf-8
# for GPIO pin usage
try:
import RPi.GPIO as GPIO
except RuntimeError:
print("Error importing RPi.GPIO!")
# what to do when exiting this pgm
import atexit
# for sleep
import time
##
## GPIO stuff
##
GPIO.setmode(GPIO.BCM) # use the GPIO names, _not_ the pin numbers on the board
# Raspberry Pi pin configuration:
# pins BCM BOARD
ledPin = 18 # pin 12
# GPIO setup
print('GPIO setup...')
GPIO.setup(ledPin, GPIO.OUT)
##
## what to do on exit pgm
##
def exitMinibot():
# GPIO cleanup
GPIO.cleanup()
##
## what to do at program exit
##
atexit.register(exitMinibot)
######
###### forever - or until ctrl+c :)
######
print('Blinking...')
while (True):
# LED ON (low active!)
GPIO.output(ledPin, GPIO.LOW)
# wait 1 second
time.sleep(1)
# LED OFF
GPIO.output(ledPin, GPIO.HIGH)
# wait 1 second
time.sleep(1)
Den dargestellten Sourcecode findet ihr übrigens hier auf GitHub. Den Schaltplan, wie die LED hier am Raspberry Pi verbunden ist, findet ihr im gleichen Repository hier. Interessant sind darin nur die LED D2 und der 370 Ω-Widerstand R2.
LED schalten als ROS-Service
Soweit so simpel. Nun zum ROS-Teil. Ich habe mich in diesem Beispiel für ein typisches Message-Service-Modell entschieden.
Service (Server)
Der Service (= Server), ist ein Python-Skript, welches auf meinem Roboter „minibot“ läuft. Dort stellt er den Service led bereit und „wartet“ es auf die Nachricht (Message) eine LED zu schalten. Hier zur Verdeutlichung der Code für den Service.
Noch ein Hinweis: Im Code gibt es einige Stellen, die auf den Hostnamen prüfen, auf dem das System läuft („minibot“). Das nutze ich, da ich einen echten Roboter habe und ein Testsystem, wo keine Hardware angeschlossen ist (nur der reine Raspberry Pi).
#!/usr/bin/env python
# coding=utf-8
# This is a service node (server)
# name of the package(!).srv
from minibot.srv import *
import rospy
# for getting the hostname of the underlying system
import socket
# showing hostname
hostname = socket.gethostname()
rospy.loginfo("Running on host %s.", hostname)
rospy.loginfo("Setting up RPi.GPIO...")
# run some parts only on the real robot
if hostname == 'minibot':
# for GPIO pin usage on the Raspberry Pi
try:
import RPi.GPIO as GPIO
except RuntimeError:
rospy.logerr("Error importing RPi.GPIO!")
else:
rospy.loginfo("NOT setup. Simulating due to other host!")
## GPIO stuff
# We use the GPIO names, _not_ the pin numbers on the board
if hostname == 'minibot':
GPIO.setmode(GPIO.BCM)
# Raspberry Pi pin configuration:
# pins BCM BOARD
ledPin = 18 # pin 12
# setting these LOW at startup
# pinListLow = (ledPin)
# setting these HIGH at startup
pinListHigh = (ledPin)
# GPIO setup
rospy.loginfo("GPIO setup...")
if hostname == 'minibot':
GPIO.setup(ledPin, GPIO.OUT)
# all pins which should be LOW at ini
# GPIO.output(pinListLow, GPIO.LOW)
# all pins which should be HIGH at ini
if hostname == 'minibot':
GPIO.output(pinListHigh, GPIO.HIGH)
# define a clean node exit
def my_exit():
rospy.loginfo("Shutting LED server down...")
if hostname == 'minibot':
# GPIO cleanup
GPIO.cleanup()
rospy.loginfo("Done.")
# call this method on node exit
rospy.on_shutdown(my_exit)
# handle_led is called with instances of LedRequest and returns instances of LedResponse
# The request name comes directly from the .srv filename
def handle_led(req):
""" In this function all the work is done :) """
# switch GPIO to HIGH, if '1' was sent
if (req.state == 1):
if hostname == 'minibot':
GPIO.output(req.pin, GPIO.HIGH)
else:
# for all other values we set it to LOW
# (LEDs are low active!)
if hostname == 'minibot':
GPIO.output(req.pin, GPIO.LOW)
# debug
rospy.loginfo("GPIO %s switched to %s. Result: %s", req.pin, req.state, req.pin)
# The name of the 'xyzResponse' comes directly from the Xyz.srv filename!
return LedResponse(req.pin)
def led_server():
# Service nodes have to be initialised
rospy.init_node('led_server')
# This declares a new service named 'led'' with the 'Led' service type.
# All requests are passed to the 'handle_led' function.
# 'handle_led' is called with instances of LedRequest and returns instances of LedResponse
s = rospy.Service('led', Led, handle_led)
if hostname != 'minibot':
rospy.logwarn("SIMULATING due the fact that the hostname is not 'minibot'.")
rospy.loginfo("Ready to switch LEDs.")
# Keep our code from exiting until this service node is shutdown
rospy.spin()
if __name__ == "__main__":
led_server()
Message
Seine Anweisungen erhält der Service über den ROS-typischen Mechanismus über das Netzwerk Nachrichten (Messages) auszutauschen. Das dazugehörige Message-File Led.srv sieht wie folgt aus:
int8 pin int8 state --- int8 result
Der Service erwartet also eine Nachricht mit einem integer-Wert für den pin, der geschaltet werden soll (das müsste korrekterweise GPIO heißen, stelle ich gerade fest), sowie 0 oder 1 (state) für LOW oder HIGH. result ist die Antwort, die zurückgegeben wird, wenn der Pin geschaltet wurde. In meinem Code entspricht die Antwort immer der Anforderung; also 1 für HIGH und 0 für LOW.
Client
Der Client ist hier der Teil, der die Nachricht an den Service (Server) übermittelt, die LED an- oder auszuschalten. Anzumerken ist vielleicht noch, dass meine LED im Schaltplan am Raspberry Pi low-aktiv ist – also bei LOW an- ist und bei HIGH ausgeschaltet. Und das geht in ROS dann so:
#!/usr/bin/env python
# coding=utf-8
# This is a client node
import sys
import rospy
# name of the package(!).srv
from minibot.srv import *
def led_switcher_client(pin):
# Service 'led' from led_server.py ready?
rospy.wait_for_service('led')
try:
# Create the handle 'led_switcher' with the service type 'Led'.
# The latter automatically generates the LedRequest and LedResponse objects.
led_switcher = rospy.ServiceProxy('led', Led)
# the handle can be called like a normal function
response = led_switcher(pin, state)
return response.result
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s", e)
def usage():
return "\nError!\n\nUsage: %s [pin] [state]\nExample to turn a LED for GPIO 18 ON: %s 18 1\n"%(sys.argv[0], sys.argv[0])
if __name__ == "__main__":
# enough arguments?
if len(sys.argv) == 3:
pin = int(sys.argv[1])
state = int(sys.argv[2])
else:
print usage()
sys.exit(1)
# call the led_switcher function
# and show the result
rospy.loginfo("GPIO %s switched to %s. Result: %s", pin, state, led_switcher_client(pin))
Wichtig:
Ich gehe an dieser Stelle nicht auf den üblichen ROS-Teil ein, wie die Dateien in ein neues ROS-Paket einzubinden sind (CMakeLists.txt usw.)! Dafür gibt es die guten fertigen ROS-Tutorials wie dieses, auf denen mein Beispiel fast 1:1 basiert. Außerdem findet ihr die Dateien mit Ordnerstruktur ja auch im GitHub-Repository im ROS-Paket minibot.
Schalten der LED mit ROS – Los geht’s!
Die Ausführung ist ziemlich simpel. Als erstes muss immer der ROS master gestartet werden. Wie üblich alles auf der Kommandozeile (Shell):
roscore
Sieht dann ungefähr so aus:
Als zweites wird der LED-Service (Server) in einer weiteren Shell gestartet mittels
rosrun minibot led_server.py
gestartet:
Als drittes und letztes erfolgt der Aufruf zum Einschalten der LED an GPIO 18 (0, weil meine LED dann ausgeht)
rosrun minibot led_client.py 18 0
oder zum Ausschalten
rosrun minibot led_client.py 18 1
Und so sieht es auf der Serverseite aus:
Alles klar? Ich hoffe, die Anleitung war einigermaßen verständlich und hilfreich. Feedback gerne über die Kommentarfunktion