Programmierung / Funktionsweise der ROS2 Codes

Programmierung / Funktionsweise der ROS2 Codes

In diesem Abschnitt wird der Ablauf der verschiedenen Programme beschrieben. Dazu gehört der die Motorsteuerung, genau sowie die Infrarotsensor Hindernisumfahrung oder die Ultraschallsensor Hindernisumfahrung. Weiter wird das Ergebnis zu den drei Teilen dargestellt.

Da es die Bibliothek wiringPi, welche für die Ansteuerung der GPIO Pins über CPP nicht mehr unterstützt wird, haben wir uns für die Python Bibliothek "RPi.GPIO" entschieden.

Da es beim Ausführen von den Codes auf dem RPi zu dem Fehler "RuntimeError: Not running on a RPi!" kam, können die folgenden Befehle helfen. Die hier genannten Befehle müssen bei jedem Neustart des RPi's erneut ausgeführt werden.

Die ersten beiden Befehle zeigen, ob gpiomem schon vorhanden ist.

Befehl: ls -l /dev/ gpiomem

Ausgabe: crw-rw---- 1 root dialout 239, 0 Apr 1 17:23 /dev/gpiomem

Befehl: groups

Ausgabe: ubuntu adm dialout cdrom floppy sudo audio dip video plugdev netdev lxd

Um gpiomem dann zu erlauben sind die folgenden zwei Befehle erforderlich.

Befehl: sudo chown root.gpio /dev/gpiomem

Befehl: sudo chmod g+rw /dev/gpiomem

Um mit der Programmierung zu beginnen, wurde ein Ros2 Workspace mit einem source Ordner erstellt.

mkdir ros_ws

cd ros_ws

mkdir src

In dem source Ordner wurde dann das ROS2 Package mit den benötigten Bibliotheksabhängigkeiten erstellt. Dazu gehören rclpy, der ROS2 Client Library für Python und std_msgs, um die Standard Nachrichten, wie Int8, Float64 oder String in ROS2 nutzen zu können.
ros2 pkg create --build-type ament_python packagename(z.B. sensor_test) --dependencies rclpy std_msgs

Motorsteuerung

Abb. 8 Motorsteuerung Ablauf

Es wurde damit begonnen die Motoren zu programmieren und zu testen. Dazu wurde eine ROS2 Node namens slave.py erstellt, welche die Programmierung zur Steuerung der Motor GPIO Pins beinhaltet. Dazu werden bestimmte Nachrichten wie forward, backward, right, left oder stop manuell an diese ROS2 Node gepublished.

Der Ablauf ist dazu in Abb. 8 Graphisch dargestellt.

Damit überhaupt etwas passiert muss zuerst der ROS2 Code, die slave.py ausgeführt werden.

ros2 run sensor_test Motortest

Unter Demos wird dazu im Detail beschrieben, wie die verschiedenen Programme ausgeführt werden können.

Danach ist der subscriber in einer Art Standby Modus und wartet auf eingehende Nachrichten.

Im nächsten Schritt sendet jemand manuell eine Movement Nachricht an den subscriber.

ros2 topic pub /cmd_vel std_msgs/msg/String '{data: forward}'

Diese Nachricht wird in einer Dauerschleife gesendet, bis das publishen gestoppt wird.

Die Node "motor_subscriber" empfängt dann die zuvor gesendete Nachricht über den dort definierten subscriber.

Anhand dieser Nachricht entscheidet die Node über eine If-Schleife welche GPIO Pins angesteuert werden müssen. Dazu gibt es 3 Pins für den linken Motor und 3 Pins für den rechten Motor. Jeweils einen für vorwärts, einen für rückwärts und einen um die Motoren zu aktivieren. Die Aktivierung erfolgt in der Initialisierungsfunktion. Bei der Motorsteuerung wurden die Geschwindigkeit auf 0% gesetzt damit der AlphaBot erst losfährt, wenn die Slave eine Nachricht erhält.

self.PWMA = GPIO.PWM(ENA,500)

self.PWMA.start(0)

Bei der Nachricht "forward" die beiden GPIO Pins für das vorwärts Fahren auf HIGH gesetzt und der AlphaBot fährt vorwärts.

Infrarot Hindernisumfahrung

Abb. 9 Infrarot Hindernisumfahrung

Nachdem die Motoren mit ROS2 implementiert und getestet wurden, wurde die Hindernisumfahrung mit Infrarotsensoren implementiert. Hierfür wurden zwei neue Dateien mit den Namen "master_IR.py" und "slave_IR.py" in dem source Ordner des ROS2 Packages erstellt.

Der Ablauf für die Hindernisumfahrung mit Infrarotsensoren ist in Abb. 9 abgebildet.

Das Programm wird mit einer ROS2 launch-file ausgeführt. Die launch-file beinhaltet alle auszuführenden ROS2-Nodes. Dazu gehört der package Name und der Name der auszuführenden Datei. Mit dem folgenden Befehl werden dann beide Nodes ausgeführt.

ros2 launch sensor_test alphabot_IR_launch.py

In der slave-Node werden den Motoren durch die Initialisierung direkt eine Geschwindigkeit zugeordnet, damit der AlphaBot beim ausführen des Codes direkt losfährt. Durch das ausführen der launch-file werden über die Slave-Node die Sensordaten an die Master-Node gepublished.

Nachdem die Master-Node die Sensordaten erhalten hat, entscheidet sie über eine IF-Schleife in welche Richtung der AlphaBot fahren kann.

Der unterschied zur Motorsteuerung oben ist, das hier der Master anhand der Sensordaten forward, right, left oder backward an die Slave-Node published.

Für die Sensordaten musste zunächst der richtige Datentype herausgefunden werden. Dazu kann dieser Code in der Slave-Node unter der timer_callback() Funktion nachdem DR_status deklariert wurde integriert werden.

print(type(DR_status))

So wurde herausgefunden, dass es sich hier um ein Integer handelt. Mit der Codezeile print(DR_status) wurde festgestellt, dass diese Infrarotsensoren bei einer 1 nichts vor ihnen steht und bei einer 0 etwas im Weg ist.

So published die Master-Node "forward" wenn die Slave-Node für beide Sensoren eine 1 published und steuert beide GPIO Pins zum vorwärts fahren an. Wenn z.B. der rechte Sensor eine "0" und der linke eine "1" angibt, dann ist rechts ein Hindernis. Der master sendet der Slave die Nachricht "left", sodass die Slave-Node die GPIO Pins zum rückwärts fahren des Linken Motors ansteuert.

In dem folgenden Sequenzdiagramm ist die Kommunikation zwischen Master und Slave deutlicher zu erkennen (Abb. 10).

So ist in der Abbildung zu sehen, das zunächst die Slave die Sensordaten abfragt und über timer_callback() an den Master published.

Anschließend published der Master anhand der erhaltenden Sensordaten ein Movement an die Slave. Die Slave steuert dann die Motoren an.

Abb. 10 Infrarot Hindernisumfahrung Sequenzdiagramm

Ultraschall Hindernisumfahrung

Zuletzt wurde die Hindernisumfahrung mit dem Ultraschallsensor implementiert. Die Motoren werden wie bei den anderen Programmen angesteuert. Der Code wird mit einem Servomotor, um den Ultraschallsensor nach rechts und links zu drehen, erweitert und die Infrarotsensoren werden durch den Ultraschallsensor ersetzt.

Dazu wurden die beiden Dateien slave_Sonar.py und master_Sonar.py erstellt.

Abb. 11 Ultraschall Hindernisumfahrung

Bei der Implementierung lag zunächst die Problem, dass die Master-Node die Sensordaten nicht zum richtigen Zeitpunkt erhalten hat. So wurden zum Beispiel die Sensordaten für rechts und links gepublished, sind aber erst im zweiten durchlauf der Master-Node bei dem subscriber angekommen.

Die funktionierenden Codes können in dem SVN-Repository unter Demo nachgeschlagen werden.

Der Ablauf für die Hindernisumfahrung mit Ultraschallsensor ist in Abb. 11 dargestellt.

Hier wurde wie bei der Infrarot-Hindernisumfahrung eine Launch-file erstellt, welche zunächst gestartet wurden muss.

ros2 launch sensor_test alphabot_Sonar_launch.py

Wenn die launch-file ausgeführt wurde, fährt der AlphaBot los und die Slave-Node published die Sensordaten über die Topic Sonar/middle an die Master-Node.

Anhand dieser Daten überprüft die Master-Node über eine if-Schleife, ob die Entfernung kleiner 50 ist. Solange der Wert über 50 ist, wird die Nachricht "forward" gepublished und der AlphaBot fährt geradeaus.

Wenn die Entfernung kleiner 50 wird, hält die Slave-Node bei erhalt der Nachricht "stop" von der Master-Node, den AlphaBot an. Weiter published die Master-Node die Nachricht "lookRight". Wenn die Slave-Node diese Nachricht erhält, steuert sie die GPIO Pins für den Servomotor an, um den Sensor nach rechts zu drehen. Das funktioniert mit dem sogenannten DutyCycle.

self.p.ChangeDutyCycle(2.5)

Dann published die Master-Node "rightData" und nachdem die Slave-Node das erhalten hat, published sie die Sensordaten über die Topic "Sonar/right".

Mit dem gleichen Prinzip wird der Sensor als nächstes nach links gedreht. Das passiert über die Nachrichten "lookLeft" und "leftData".

Zuletzt wird der Ultraschallsensor mit der Nachricht "lookForward" und dem entsprechendem DutyCylce 7.5 zur Ausgangsposition zurückgedreht.

Die Master-Node wertet die Daten über eine weitere if-Schleife aus und published die Nachricht, in welche Richtung der AlphaBot fahren kann.

So wird "backward" gepublished, wenn links und rechts der Wert kleiner 50 ist, oder "right", wenn der rechte Wert größer ist wie der linke Wert. Und ansonsten wird "left" gepublished.

Die Slave-Node steuert anhand der erhaltenden Nachricht die GPIO Pins an, um das Hindernis zu umfahren.

In dem hier gezeigten Sequenzdiagramm wird die Kommunikation zwischen Master und Slave aufgezeigt (Abb.12).

So ist zusehen, wann die Slave Sensordaten an den Master published und wann der Master Movements an die Slave published.

Abb. 12 Ultraschall Hindernisumfahrung Sequenzdiagramm

→ zurück zum vorherigen Artikel: Kommunikation Raspberry Pi mit ROS2