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