Vorüberlegungen zur Implementierung

Wir brauchen:

  • Eine Karte mit allen Wänden
  • Funktionen die uns:
    • Ein Bild von der Kamera liefern
    • Die Begrenzungen des Labyrinths in Form von 4 Eckpunkten zurück liefern
  • Eine Liste aller gefunden Roboter mit Name, Position (als Zuordnung zu einem bestimmten Feld sowie als pixelgenaue Position innerhalb des Labyrinths, um eine kontinuierliche Bewegung des Roboters in der Karte darzustellen)
  • Auf die Daten (Karte, Positionen) muss von einem Python-Programm (2D-Karten-Visualisierung), als auch von einem C++ Programm (3D-Karte) zugegriffen werden können.
  • Es sollte möglich sein, die Kartendaten an mehrere Roboter über Funk (mehrere Serielle Schnittstellen) zu senden, während gleichzeitig die Karte am PC dargestellt wird.

Fragen:

  • Shared Library oder einfach Unterordner lib/ dessen Dateien alle Programme mit einbeziehen?
  • Wozu brauche ich ROS, um die Bilder einfach durch noch eine Bibliothek durchzuschleifen und das compilieren unnötig zu komplizieren?
  • Eigene C++ Klasse Ueye mit init(), openCam(), und getImage()?
  • Wie soll das Python Programm zur Darstellung an die Kartendaten kommen?
  • Wie soll die in C++ implementierte 3D-Karte an die Kartendaten kommen?
  • Wie wird die Karte zu den Robotern gesendet?
    • Einfacher Server? Boost Asio kann sowohl mit Sockets arbeiten als auch mit seriellen Schnittstellen
    • Python Programm muss kaum angepasst werden, egal ob es die Kartendaten vom Serialport oder von localhost:9999 liest.
    • An jedem Arbeitsplatzrechner können ein oder mehrere Python Programme laufen, die die Kartendaten per XBee an einen oder mehrere Roboter senden
    • Auch das 3D-Kartenprogramm kann sich gleichzeitig zum Server verbinden und bekommt eine Karte
  • Lokale oder systemweite Installation?
    • Mit CMake ist beides ohne Probleme auch gleichzeitig möglich

Plan:

  • Einfache C++ Shared Library liblaustracker.so schreiben
  • Vorhandene libueyecam zum Zugriff auf die Kamera nutzen
  • Daten zwischen den Programmen über ROS austauschen

Die Laustracker Programme

Einige Beispiele zum Aufruf der einzelnen Programme mit verschiedenen Kommandozeilenoptionen zur Nutzung von Beispielbildern von der Festplatte und Liveaufnahmen von der Kamera befinden sich in der Datei laustracker/ros/README.

Laustracker Camnode

Der Laustracker Camnode ist ein ROS Node, der über verschiedene ROS Services Funktionen der Kamera zur Verfügung stellt, wie zum Beispiel das Einstellen der Belichtungszeit oder aktuelle Kamerabilder.

Gestartet wird der Kamera-Node mit dem Befehl:

~]$ rosrun ltcamnode laustracker-camnode

Natürlich muss im Hintergrund bereits roscore laufen, der die Nachrichten zwischen den einzelnen ROS-Nodes verteilt:

~]$ roscore

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl rosrun ltcamnode laustracker-camnode --help ausgeführt.

Befehl: laustracker-camnode

--help

Anzeige alle unterstützen Optionen.

--fps=fps

Angabe der Framerate, die verwendet werden soll. Es sollte eine realistische Framerate gewählt werden, die auch von der Hardware unterstützt wird, ansonsten läuft der Camnode mit der maximal möglichen Framerate, die auch schwanken kann.

--id=camera_id

Angabe einer Kamera Nummer, wenn nicht Kamera 1 verwendet werden soll.

Laustracker Server

Der Laustracker-Server verrichtet den Großteil der Arbeit. Er nutzt die liblaustracker-Bibliothek zum Finden des Labyrinthes, der Wände und der Roboter und stellt in einer Endlosschleife stets die aktuellen Roboterpositionen und eine Karte über ROS Services zur Verfügung.

Manueller Modus

Im manuellen Modus wird der Laustracker-Server über folgenden Befehl gestartet:

~]$ rosrun ltserver laustrackerd \
      --camera-node ueye-camera

Oder ohne die Verwendung des Kamera-Node:

~]$ rosrun ltserver laustrackerd
alternate text

Abbildung 1: Fenster, das sich nach dem Starten vom Laustracker-Server ltserver öffnet.

Durch Drücken der Taste m kann die Position des Labyrinthes manuell festgelegt werden, durch Drücken der Taste a sollte die Labyrinthposition automatisch erkannt werden und durch Drücken der Taste e wird das Labyrinth über rote Eckmarkierungen lokalisiert.

alternate text

Abbildung 2: Manuelles Festlegen der Labyrinthposition durch Klicken auf die Eckpunkte.

Durch Drücken der ESC Taste innerhalb des Fensters [please_click_on_the_labyrinth_edges] wird die Position übernommen. Ansonsten wird standardmäßig durch Drücken der ESC Taste das komplette Programm beendet.

alternate text

Abbildung 3: Finden der Labyrinthposition über rote Eckmarkierungen.

Anschließend werden die Wände durch Drücken der Taste w lokalisiert:

alternate text

Abbildung 4: Lokalisieren der Wände.

Jetzt wird durch Drücken der Tasten + und - die Belichtungszeit so eingestellt, dass nur noch die LEDs vom Roboter gut erkennbar sind oder man drückt die Taste d, um gleich die voreingestellte Belichtungszeit zum Finden der Roboter zu wählen. Anschließend kann durch Drücken der Taste r nach Robotern im Bild gesucht werden. Wenn die Roboter nicht zuverlässig erkannt wurden, kann die Belichtungszeit auch noch nachgeregelt werden.

alternate text

Abbildung 5: Kurze Belichtungszeit zum Finden der LEDs am Roboters

Alle unterstützten Tasten werden auch auf der Konsole mit kurzem Hilfstext angezeigt, wenn der Laustracker-Server im manuellen Modus gestartet wird.

Jeder Arbeitsplatzrechner, auf dem ROS und die Netzwerkverbindung richtig konfiguriert ist, kann sich jetzt einfach mit dem Kartenserver verbinden, die Karte abfragen, das 2D- oder 3D-Visualisierungsprogramm starten oder die Kartendaten in einem Python Skript weiterverarbeiten und dann über ein angeschlossenes XBee Modul an einen der Roboter senden.

Automatischer Modus

Über die Kommandozeilenoptionen --auto wird der automatische Modus gestartet, über den das Programm die oberen Schritte der Reihe nach ohne Benutzerinteraktion ausführt. Wenn es aber einmal zu Problemen kommen sollte, kann man im manuellen Modus besser herausfinden, an welcher Stelle es Probleme gibt.

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl rosrun ltserver laustrackerd --help ausgeführt.

Befehl: laustrackerd

--help

Anzeige aller unterstützen Optionen.

--debug

Zwischenbilder und Debug-Ausgaben anzeigen.

--no-undistort

Wenn das Kamerabild nicht entzerrt werden soll, weil zum Beispiel schon entzerrte Bilder von der Festplatte benutzt werden.

--mask

Gibt an ob eine Maske zum Finden der Wandpositionen benutzt werden soll.

--manual

Wenn die Labyrinthecken manuell ausgewählt werden sollen.

--reddots

Wenn rote Markierungen an den Labyrinthecken genutzt werden sollen, um die Labyrinthposition zu finden.

--walls-img=image

Angabe eines Bildes zum Finden der Wände

--robots-img=image

Angabe eines Bildes zum Finden der Roboter

--robots-vid

Angabe eines Videos in dem die Roboter in einer Endlosschleife gesucht werden sollen.

--camera-node=name

Angabe des Namens eines Kamera-Node, der zum Beziehen der Kamerabilder genutzt werden soll.

--robots-conf=filename

Nutzung einer speziellen Konfigurationsdatei, die Konfigurationsdaten zum Finden der Roboter enthält.

--labyrinth-conf=filename

Nutzung einer speziellen Konfigurationsdatei mit Informationen zum Labyrinth anstelle der Default-Werte.

--debuglevel=number

Angabe eines Debug-Levels, dass an die Laustracker Bibliothek weitergereicht wird.

--auto

Starten des automatischen Modus.

--headless

Headless-Modus, keine Bilder anzeigen.

--no-wrap

Keine perspektivische Transformation durchführen, das erhöht die Framerate.

--fast

Einige Optimierungen nutzen um die Framerate zu erhöhen, es kann etwas ungenauer sein, die Verwendung dieser Option ist aber generell zu empfehlen.

--load

Labyrinthposition und Wände automatisch, aus vorher im manuellen Modus mit der Taste s gespeicherten Karte, laden. Dies kann nützlich sein, wenn abends aufgrund des schlechten Lichtes der Leuchtstoffröhren die Wände nicht mehr richtig erkannt werden.

--fps=fps

Angabe der Framerate die verwendet werden soll, kann nicht schneller als die Framerate der Kamera sein.

Laustracker2D

Wenn der Laustracker-Server im Hintergrund läuft, kann das 2D-Visualisierungsprogramm laustracker2D gestartet werden:

~]$ rosrun laustracker2d laustracker2d.py

Bei laustracker2D handelt es sich um ein Python Script, dass Pygame zur grafischen Darstellung nutzt.

alternate text

Abbildung 6: Python Script laustracker2D

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl rosrun laustracker2d laustracker2d.py --help ausgeführt.

Befehl: laustracker2D.py

-h, --help

Einen Hilfstext anzeigen und danach beenden.

-d, --debug

Debug Ausgaben und Status Nachrichten in der Konsole anzeigen.

-p, --pos

Die Position des ersten gefunden Roboters mit cm-Angaben im Fenstertitel anzeigen.

Laustracker3D

Laustracker3D ist mit OpenSceneGraph umgesetzt und nutzt OpenGL zur Darstellung des 3D-Modells vom Labyrinth.

Ebenso wie Laustracker2D bezieht es die Kartendaten über ROS vom Laustracker-Server.

~]$ rosrun laustracker3d laustracker3d
alternate text

Abbildung 7: Laustracker3D

Hier noch ein kurzes Demonstrationsvideo unter Nutzung von robotsvideo.avi:

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl rosrun laustracker3d laustracker3d --help ausgeführt.

Befehl: laustracker3D

-w

Im Fenstermodus starten, nicht Vollbild.

-n

Keine relativen Roboterpositionen nutzen, sondern Roboter immer mittig im momentanen Bezugsfeld anzeigen.

-p

Debug Ausgaben in der Konsole ausgeben (print).

-d

Debug Ausgaben am Bildschirm anzeigen (draw).

-t

Eine Testkarte laden.

Laustracker XBee Node

Insgesamt gibt es zwei Laustracker-XBee-Nodes, einen etwas umfangreicheren (ltxbee.py) und ein Minimalbeispiel (ltxbee-min.py). Beide können sich mit dem Laustracker-Server verbinden und dann Kartendaten oder Roboterpositionen über Funk (XBee) an die Roboter senden.

Hier folgen ein paar Möglichkeiten zum Starten des XBee-Node:

~]$ rosrun ltxbee ltxbee.py -r Andi
~]$ rosrun ltxbee ltxbee.py -r Pete -p /dev/ttyUSB1
~]$ rosrun ltxbee ltxbee-min.py

Funktionsweise des minimalen XBee Nodes

Gehen wir als erstes mal den Quelltext von ltxbee-min.py Stück für Stück durch.

In den ersten 5 Zeilen werden ein paar generelle Python Module importiert, wie zum Beispiel serial, das Funktionen zum Zugriff auf den Serialport bietet, die später zur Kommunikation mit dem XBee Modul über USB benötigt werden.

import sys
import serial
import roslib
roslib.load_manifest('ltxbee')
import rospy
from ltserver.msg import Map, Robots

Anschließend werden noch die Definitionen für die ROS Nachrichten Map und Robots vom Laustracker-Server importiert, die zum Empfangen und Auswerten des Labyrinthaufbaus und der Roboterpositionen notwendig sind.

In der nächsten Zeile wird der Name unseres Roboters festgelegt, hier Andi, somit können wir uns später aus der Liste der Roboter nur den Andi heraus greifen. Außerdem sind an den Abseitsplatzrechnern spezielle UDEV-Regeln eingerichtet, so dass den XBee Modulen feste Gerätenamen unter Linux zugewiesen werden, wie zum Beispiel /dev/andi/xbee für Andi.

# Change this to the name of your robot
robot_name = "Andi"

In den folgenden zwei Zeilen werden nur 2 globale Variablen initialisiert, wenn debug auf True gesetzt wird, werden zusätzlich ein paar Debug-Ausgaben in der Konsole angezeigt

debug = True
serialport = None

Danach kann der Gerätename des Serialports festgelegt werden, er besteht aus 3 Teilen:

  • /dev/ Die meisten Geräte (devices) bekommen unter Linux Dateien im Unterverzeichnis /dev/ zum einfachen Zugriff zugewiesen, streng nach der Unix-Philosophie “alles ist eine Datei”.
  • robot_name.lower() Alle Buchstaben im Roboternamen werden in Kleinbuchstaben umgewandelt.
  • /xbee Der Name wurde einfach so in der UDEV-Regel festgelegt.
port = "/dev/" + robot_name.lower() + "/xbee"

Die Funktion robotsMsgCallback() wird jedes mal aufgerufen, wenn wir vom Laustracker-Server eine neue Nachricht bekommen, dass kann durchaus 30 mal pro Sekunde passieren, daher sollten in der Funktion keine zeitaufwendigen Operationen ausgeführt werden. Außerdem wird die Funktion asynchron zur While-Schleife in Main ausgeführt, so dass es Probleme beim gleichzeitigen Zugriff auf Variablen geben kann oder das Print-Ausgaben, die nicht aus einem einzigen Befehl bestehen, zerstückelt werden, wenn gleichzeitig etwas in Main ausgegeben wird. In ltxbee.py sind diese Probleme bereits gelöst.

In einer For-Schleife wird dann über die Liste der empfangenen Roboter iteriert und überprüft, ob der Robotername mit dem Namen unseres Roboters übereinstimmt. Wenn das der Fall ist, schreiben wir eine Zeichenkette in den Serialport, die dann automatisch über XBee an unseren Roboter gesendet, und im Laustracker Test-Menü auf dem LCD des Roboters angezeigt wird.

Das Newline Symbol \n veranlasst das Programm auf dem Roboter, die zweite Zeile des LCD zu löschen, so dass neue Daten angezeigt werden können, dann kommt eine sich öffnende eckige Klammer und dann das: str(int(robot.absolut_pos_cm.x)).

Wir gehen am besten von innen nach außen vor. Wir haben den aktuellen Roboter robot als Schleifenvariable, davon greifen wir auf die absolute Roboterposition in cm .absolut_pos_cm und davon auf die X-Koordinate .x zu. Da das ein Fließkomma Wert ist, wir aber nur ein 16 Zeichen breites Display auf dem Roboter haben und dementsprechend nicht genug platzt, schneiden wir alle Kommastellen ab und wandeln die Zahl in einen Integer int(...). Jetzt haben wir beispielsweise die Zahl 15, wenn der Roboter im ersten Feld 15 cm vom Koordinatenursprung entfernt steht. Damit diese auf dem Roboterdisplay angezeigt werden kann, muss sie noch in eine 2 Byte lange ASCII Zeichenkette umgewandelt werden. Das erledigt die Funktion str(...). Die anderen Teilstücke erfolgen analog.

def robotsMsgCallback(data):
    # Callback function that gets called when we receive a new
    # robots message from the Laustracker server
    if debug:
        rospy.loginfo(rospy.get_caller_id() + " I heard:\n%s\n",data)
    for robot in data.robots:
        if robot.name == robot_name:
            # Display some text on the Robolaus lcd
            serialport.write("\n[" +
                            str(int(robot.absolut_pos_cm.x)) + "," +
                            str(int(robot.absolut_pos_cm.y)) + "] " +
                            str(int(robot.angle)) + " " +
                            str(int(robot.field_idx.x)) + " " +
                            str(int(robot.field_idx.y)))

Im Hauptteil des Programms wird zuerst versucht den Serialport mit einer Baudrate von 57600 zu öffnen.

if __name__ == "__main__":

    try:
        print("Trying to open the serial port: " + port)
        serialport = serial.Serial(port, baudrate=57600, timeout=0.1)
        serialport.flushInput()
    except:
        print("Error opening serial port")
        serialport = None

Dann kommt eine ROS spezifische Initialisierung des Nodes und wir sagen ROS, dass wir Robots-Nachrichten vom Laustracker-Server empfangen wollen und dass die empfangenen Daten der oben schon beschriebenen Funktion robotsMsgCallback() übergeben werden sollen.

rospy.init_node('ltxbee', anonymous=True)

rospy.Subscriber("robots_msg_publisher", Robots, robotsMsgCallback)

Anschließend lesen wir in einer Endlosschleife Daten vom Serialport und geben diese in der Konsole aus.

# endless loop
while not rospy.is_shutdown():
    rospy.sleep(0.5)

    try:
        tmp = serialport.read(1000)
        if tmp:
            # sys.stdout.write(tmp)
            rospy.loginfo("\nReceived: " + tmp)
    except:
        print("Error reading from serial port")

Auf dem SVN Server der Professur befindet sich jetzt noch im robolaus2 Verzeichnis unter moduleSensor/beispiele/Test_Menu/ ein Testprogramm dass genutzt werden kann, um die Roboterposition auf dem LCD anzuzeigen.

Ich gehe hier nur auf die test_laustracker() Funktion ein, die unter dem Menüpunkt Laustracker verfügbar ist.

Zuerst wird das LCD gelöscht und ein kleiner Hinweistext ausgegeben:

//****************<menu-Laustracker>***********************
const prog_char str_menu_laustracker_text_00[] = "Laustracker";
const prog_char str_menu_laustracker_text_01[] = "LED   <on> <off>";

void test_laustracker(void) {

  uint8_t c;

  lcdclr();
  lcdstr_p(str_menu_laustracker_text_00);
  lcdxy(0,1);
  lcdstr_p(str_menu_laustracker_text_01);

Danach wird über die XBee Schnittstelle über Funk eine Willkommensnachricht gesendet, die danach im Laustracker-XBee-Node in der Konsole ausgegeben wird.

uart_select(kUartXbee);
serstr1_p(PSTR("Connected to Laustracker XBee "));

Anschließend werden in einer Endlosschleife die 3 Knöpfe am Roboter abgefragt, wenn der linke rote Knopf kButtonLeft gedrückt wird, wird das Menü wieder verlassen, beim Drücken des mittleren Knopfes kButtonMiddle werden die Laustracker LEDs eingeschaltet und beim Drücken des rechten Knopfes kButtonRight wieder ausgeschaltet.

while (1) {
  if (buttons_get(kButtonLeft)) {
    leds_set(kLedLeft, kLedOn );
    while (buttons_get(kButtonLeft)) { mdelay(100);}
    leds_set(kLedLeft, kLedOff);
    return;
  }

  if (buttons_get(kButtonMiddle)) {
    leds_set(kLedLaustracker, kLedOn);
    leds_set(kLedMiddle, kLedOn);
    mdelay(100);
  }

  if (buttons_get(kButtonRight)) {
    leds_set(kLedLaustracker, kLedOff);
    leds_set(kLedMiddle, kLedOff);
    mdelay(100);
  }

Solange Daten im XBee Buffer verfügbar sind (serstat1() != 0), lesen wir ein Byte nach dem anderen, wenn wir \n empfangen, löschen wir das Display, bei \r machen wir nichts, ansonsten geben wir das Byte als ASCII Zeichen, so wie es ist, auf dem LCD aus.

    if(serstat1() != 0)
    {
      c = serinp1();
      switch (c) {
        case '\r':
          break;
        case '\n':
          lcdclr();
          lcdstr_p(str_menu_laustracker_text_00);
          lcdxy(0,1);
          break;
        default:
          lcdout(c);
      }
    }
  }
}

Da die Daten vom Roboter nicht weiter verarbeitet werden, muss das Testprogramm nicht geändert werden, wenn andere Daten angezeigt werden sollen. Allerdings werden die Daten auch noch nicht ausgewertet.

Zur Auswertung der Daten wäre die Übermittlung als Bytes und nicht als ASCII Zeichenkette sinnvoller, aber das lässt sich leicht im Python Script anpassen.

In Abbildung 8 wird dann der Reihe nach folgendes auf dem LCD in der zweiten Zeile angezeigt: [Position_in_cm_x,Position_in_cm_y] Winkel_in_Grad Feldindex_x Feldindex_y.

Der Roboter steht mittig, 15 cm in X-Richtung und 15 cm in Y-Richtung vom Koordinatenursprung entfernt im Feld (0,0) und blickt in Richtung Norden, der Orientierungswinkel beträgt 1°.

alternate text

Abbildung 8: Anzeige der Position auf dem Roboter Display mit ltxbee-min.py.

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl rosrun ltxbee ltxbee.py --help ausgeführt.

Befehl: ltxbee.py

-h, --help

Einen Hilfstext anzeigen und danach beenden.

-d, --debug

Debug Ausgaben und Status Nachrichten in der Konsole anzeigen.

-r ROBOT, --robot=ROBOT

Angabe des Namens des Roboters, der verfolgt werden soll und dessen Daten per XBee versendet werden sollen.

-p PORT, --port=PORT

Angabe des Serialports, wenn nicht /dev/<robotername>/xbee benutzt werden soll.

Befehl: ltxbee-min.py

Momentan ohne Unterstützung für Kommandozeilenoptionen da Minimalbeispiel.

Laustracker CLI

Das Programm laustracker-cli bietet etwa den gleichen Funktionsumfang wie der Laustracker-Server nur ohne ROS-Unterstützung, und ist dadurch leichter zu debuggen.

Einige Hinweise zur Anwendung und Beispiele zu den unterstützten Kommandozeilenoptionen befinden sich in der Datei laustracker/src/cli/README.

Weiterhin wurde es im Kapitel 4.3 Teilschritte oft zu Demonstrationszwecken benutzt.

Die Laustracker-Bibliothek enthält auch noch eine Funktion zur Ausgabe der Karte auf der Konsole, wie im folgenden Ausschnitt zu sehen ist:

#########################
#              #        #
#  ##########  ##########
#  #   <    #           #
#  #  ################  #
#  #  #         ^ #  #  #
#  #  #  #######  ####  #
#     #  #     #        #
###   #  ####  ##########
#     #     #           #
#  #######  #############
#  #     #              #
#  #  #######  #######  #
#  #        #  #     #  #
#  ######   #  #  #  #  #
#           #     #  #  #
#########################

Unterstützte Optionen

Zum Anzeigen der unterstützten Kommandozeilenoptionen wird der Befehl laustracker-cli --help ausgeführt.

Befehl: laustracker-cli

--help

Einen Hilfstext inklusive aller unterstützen Optionen anzeigen.

--debug

Zwischenbilder und Debug-Ausgaben anzeigen.

--no-undistort

Wenn das Kamerabild nicht entzerrt werden soll, weil zum Beispiel schon entzerrte Bilder von der Festplatte benutzt werden.

--mask

Gibt an, ob eine Maske zum Finden der Wandpositionen benutzt werden soll.

--black

Schwarze Linien am Boden zum Lokalisieren der Wände benutzen.

--manual

Wenn die Labyrinthposition manuell ausgewählt werden soll.

--reddots

Wenn rote Markierungen an den Labyrinthecken genutzt werden sollen, um die Labyrinthposition zu finden.

--walls-img=image

Angabe eines Bildes zum Finden der Wände

--robots-img=image

Angabe eines Bildes von den Robotern bei geringer Belichtungszeit und eingeschaltetem LED Modul, dass zum Lokalisieren der Roboter verwendet werden soll.

--robots-conf=filename

Nutzung einer speziellen Konfigurationsdatei, die Konfigurationsdaten zum Finden der Roboter enthält.

--labyrinth-conf=filename

Eine andere Konfigurationsdatei für das Labyrinth verwenden.

--debuglevel=number

Angabe eines Debug-Levels, dass an die Laustracker Bibliothek weitergereicht wird.

--benchmark

Das Bild mit den Robotern, das über --robots-img angegeben wurde, zum Messen der maximal möglichen Framerate benutzen.

--no-wrap

Keine perspektivische Transformation durchführen, das erhöht die Framerate.

--fast

Einige Optimierungen nutzen um die Framerate zu erhöhen, es kann etwas ungenauer sein, die Verwendung dieser Option ist aber generell zu empfehlen.

Aufbau und Funktionsweise

Zusammenhang der ROS Nodes

Über das zu ROS gehörende Programm rxgraph kann man sich die einzelnen ROS-Nodes, die gerade miteinander kommunizieren, grafisch darstellen lassen. In Abbildung 9 ist eine aufbereitete Grafik zu sehen.

alternate text

Abbildung 9: Die einzelnen ROS-Nodes

Der Kamera-Node uEye camera bekommt die Kamerabilder über USB von der uEye Kamera über dem Labyrinth, und stellt sie dann als ROS Messages im Netzwerk zur Verfügung.

Der Laustracker-Server verbindet sich mit dem Kamera-Node, empfängt über den image publisher die Kamerabilder und wertet sie aus. Anschließend stellt er seinerseits eine Karte vom Labyrinth (map publisher), eine Liste der Roboter (robots publisher) und den Kameraausschnitt, in dem sich das Labyrinth befindet (camview image publisher), als ROS-Services zur Verfügung.

Laustracker3D, Laustracker2D und der Laustracker-XBee-Node verbinden sich dann mit dem Laustracker-Server und werten empfangene ROS-Messages aus, stellen die empfangenen Daten graphisch dar, oder leiten sie via XBee an die Roboter weiter.

Das Koordinatensystem

Das Koordinatensystem ist in Abbildung 10 dargestellt, der Koordinatenursprung befindet sich wie in der Computer Grafik üblich in der linken oberen Ecke und ist mit einem roten Punkt markiert.

Die kleinen roten Kreise stellen Feldmittelpunkte dar und werden zur Zuordnung der Roboter zu einem Bezugsfeld und zur Bestimmung der relativen Roboterposition in Vielfachen der Feldbreite genutzt, indem überprüft wird, zu welchem Feldmittelpunkt der Roboter den geringsten Abstand hat.

Das oberste linke Feld hat beispielsweise den Index (0,0), im Feldmittelpunkt beträgt die absolute Position 15 cm in X-Richtung und 15 cm in Y-Richtung und die relative Position ist (0.0,0.0), dieser Sachverhalt ist auch in Abbildung 8 dargestellt.

alternate text

Abbildung 10: Das Laustracker Koordinatensystem

Alle Positionsangaben werden intern als Fließkomma Zahlen (float) mit Maßeinheit cm gespeichert.

Beispielmessungen

Zur Überprüfung der berechneten Roboterpositionen der Laustracker-Software wurde an 8 Positionen stichprobenartig nachgemessen. Die einzelnen Positionen sind in Abbildung 11 dargestellt und die angezeigten Werte, sowie die Messwerte, sind in Tabelle 4.2 aufgeführt.

alternate text

Abbildung 11: Roboter Positionen bei den Beispielmessungen

Wie in Abbildung 12, in der der Roboter an Position 5 steht, zu sehen ist, macht es wenig Sinn zu versuchen, die Roboterposition genauer als auf einen halben Zentimeter zu messen. Außerdem ist auf dem Display zu sehen, dass der Roboter nicht mehr in Richtung Norden, wie in Abbildung 8 blickt, sondern mit nahezu 180° in Richtung Süden.

alternate text

Abbildung 12: Roboter an Position 5

Beispielmessungen der Roboter Position
Position Feldindex Display [cm] Real [cm]
1 0, 0 15, 15 15.5, 15.5
2 2, 0 91, 16 90.9, 15.9
3 7, 0 226, 15 226, 15
4 3, 1 106, 46 106, 46.5
5 3, 4 105, 138 105, 138
6 0, 7 16, 226 16, 226.5
7 6, 6 196, 196 196.5, 197
8 7, 7 227, 226 226, 226

Absolute und relative Roboterpositionen

Neben der absoluten Roboterposition in Pixel und cm wird vom Laustracker Server noch die relative Position zum Bezugsfeld berechnet, die unter anderem von Laustracker2D und Laustracker3D zur grafischen Darstellung genutzt wird und eventuell auch für die spätere Navigation per Karte durch das Labyrinth nützlich ist.

Zum besseren Verständnis habe ich mal mit Matplotlib zwei Diagramme in Abbildung 13 und Abbildung 14 vorbereitet.

Zum Erstellen der Diagramme wurde das Testvideo testdata/robotsvideo.avi genutzt, indem erst der Laustracker-Server mit dem Testvideo gestartet wird:

~]$ rosrun ltserver laustrackerd --walls-img=\
      ${LAUSTRACKER_PATH}/testdata/aoi_labyrinth_empty.jpg \
      --robots-vid=${LAUSTRACKER_PATH}/testdata/robotsvideo.avi  \
      --fast --fps=20

Von der Probefahrt existiert auch ein YouTube Video mit dem Titel “Laustracker3D using robotsvideo.avi as input”, somit kann man auch die Position im Diagramm mit der 3D-Darstellung und dem Kameravideo vergleichen.

Zum Aufzeichnen der Roboterposition wird, während der Laustracker-Server im Hintergrund läuft, ein weiteres kleines Python Script namens ltlog.py gestartet, welches die ROS Messages mit den Roboterpositionen empfängt und diese in eine CSV-Datei schreibt.

~]$ rosrun ltlog ltlog.py

Danach wird das Python Script plot-log.py ausgeführt, das die Messdaten aus der CSV-Datei verarbeitet und mit Hilfe der Matplotlib graphisch darstellt.

~]$ rosrun ltlog plot-log.py

In Abbildung 13 wurde pro Frame ein Punkt an der Position eingezeichnet, an der der Roboter erkannt wurde. Ich habe extra das Zeichnen einer durchgehenden Linie deaktiviert, so kann man sehen, dass durchaus noch Verbesserungspotential vorhanden ist, denn an den Stellen an denen die Linie Lücken aufweist, ist eins der folgenden Dinge passiert:

  • Eine LED wurde kurzzeitig nicht erkannt, das konnte ich schon überprüfen. Das trifft an ein paar der größeren Lücken für die weiße LED zu und kann durch vergrößern der Farbbereiche im HSV Farbraum für die weiße LED korrigiert werden.
  • Die Framerate bei der Videoaufnahme ist kurzzeitig eingebrochen oder die Kamera hat ein paar Frames verworfen, aber der Roboter ist trotzdem weitergefahren.
  • Generell ist die gepunktete Linie dadurch entstanden, weil der Roboter in dem Video relativ schnell gefahren ist und die uEye Kamera an meinem Laptop nur rund 20 Bilder pro Sekunde schafft.
alternate text

Abbildung 13: Absolute Roboterposition

In Abbildung 14 ist dann zusätzlich noch die relative Roboterposition auf der Z-Achse abgetragen. Es lässt sich leicht erkennen, dass sie zwischen -0,5 und +0,5 schwankt, was auch logisch ist. Denn der Roboter kann nicht weiter als 0,5 mal die Feldbreite vom Feldmittelpunkt entfernt sein, denn dann befindet er sich schon im nächsten Feld.

Wenn man den Anfang der grünen und roten Linie im linken Bereich des Diagramms betrachtet, sieht man, dass beide Linien ein kleines Stück unterhalb der blauen Nulllinie beginnen. Das liegt daran, dass der Roboter nicht genau mittig im Feld steht wie auch in Abbildung 13 zu erkennen ist.

Danach fährt der Roboter parallel zur X-Achse, somit verläuft die grüne Linie, die der relativen X-Position entspricht nahezu in Nullebene.

Die rote Kurve steigt dagegen bis zum Erreichen der ersten Feldgrenze an, da sich der Roboter in Y-Richtung vom Feldmittelpunkt entfernt. Bei Überschreiten der Feldgrenze springt sie auf -0.5, da sich der Roboter nun maximal vom nächsten Bezugsfeld entfernt befindet und nimmt dann annähernd linear zu und erreicht im nächsten Feldmittelpunkt wieder Null.

alternate text

Abbildung 14: Absolute und relative Roboterposition

Die relative Roboter Position könnte man jetzt zum Beispiel nutzen, um durch das Labyrinth zu fahren ohne anzustoßen. Wenn der Roboter in Richtung Norden (0 Grad) fährt, muss nur darauf geachtet werden, dass der Betrag der relativen X-Position so klein wie möglich bleibt. Wenn der Wert kleiner Null ist, muss etwas mit dem rechten Rad gegen gelenkt werden und wenn der Wert größer Null ist, muss etwas mit dem linken Rad gegen gelenkt werden und um den Roboter mittig in einem Feld zu platzieren, müssen beide Werte auf Null gebracht werden.

Die Laustracker Bibliothek ordnet dem Roboter automatisch auch noch eine diskrete Blickrichtung zu, die einen von 4 Werten haben kann P_TOP, P_BOTTOM, P_LEFT oder P_RIGHT, somit muss nur überprüft werden, ob die Blickrichtung P_TOP ist.

Implementierung

Ich habe nach der bestmöglichen Implementierung zur Lösung der einzelnen Probleme gesucht, wobei im Vordergrund stand, den Quelltext möglichst klein und verständlich zu halten und ihn gut zu strukturieren. Das heißt, verschiedene Aufgaben in verschiedene Klassen und auf verschiedene Dateien zu verteilen, so dass man leicht neue Funktionen hinzufügen kann.

Weiterhin brauchte ich Möglichkeiten, um alle Zwischenschritte zu debuggen, so dass man bei eventuell auftretenden Problemen schnell die Ursache finden kann, nachdem man zum Beispiel Funktionen der liblaustracker Bibliothek verändert hat und man nicht vor einer Blackbox sitzt, bei der man erst einmal den Quelltext editieren muss, um an Debuginformationen zu kommen. Die Anzeige von Zwischenbildern, die sich über die Optionen --debug und --debuglevel steuern lässt, eignet sich außerdem gut zur Erstellung von Grafiken zur Dokumentation.

Insgesamt ist mir die Implementierung der Software ziemlich gut gelungen, und ich bin mit dem Endergebnis zufrieden.