MasterarbeitLaTeX/tex/4_Umsetzung.tex

\chapter{Umsetzung}

Bei der Umsetzung des geplanten Systemaufbaus kam es zu mehreren Problemen, welche den geplanten Systemaufbau, sowie dessen Komplexität, negativ beeinflussen.

Die Kommunikation zwischen dem Actormodell und dem Behavior Tree musste in mehrere Komponenten aufgeteilt werden, um Konflikte innerhalb der Simulationssoftware zu vermeiden.

Zudem ist die Bewegungsplanung mit MoveIt2 deutlich komplexer als vorerst angenommen.
Diese Komplexität entsteht aus der Interaktion mehrerer Komponenten, welche das Gesamtsystem zur Ausführung benötigt.
Alle Einzelsysteme mussten hierfür konfiguriert werden, um die Kommunikation dieser zu ermöglichen.

Anhand der hier genannten Änderungen wurde die Übersicht des Systems angepasst, welche in Abbildung \ref{umsetzung_overview} zu sehen ist.

\begin{figure}[h]
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Übersicht.drawio}
\centering
\caption{Visualisierung des überarbeiteten Konzepts}
\label{umsetzung_overview}
\end{figure}

\section{Verwendete Datentypen}
In der Implementation werden unterschiedliche Datentypen verwendet.
\begin{description}
  \item[geometry_msgs::msg::Pose]
  ist einer der häufigsten Datentypen im Umgang mit Gazebo. Dieser Datentyp enthält die Information über die Lage und Rotation eines Objekts im Raum.
  Diese werden als relative Werte im Bezug auf das übergeornete Objekt gespeichert.
  Objekte wie der Mensch liegen in der Hierarchie der Simulation direkt unter der Welt, welche sich direkt im Nullpunkt und ohne Rotation befindet.
  Dadurch sind zum Beispiel die Koordinaten des Menschen absolut, da diese nicht durch die Welt verschoben und rotiert werden.
  \item[Area]
  ist eine Datenstruktur mit einem Vektor an Positionen, welche eine Zone im Zweidimensionalen Raum definieren.
  Jede Position ist eine einfache Datenstruktur aus 2 Gleitkommazahlen für je die X- und Y-Koordinate der Position.
  Der Verwendungszweck dieser Struktur ist die einfache Definition von Zonen, welche für Positionsgenerierungen und Postionsabfragen genutzt werden können.
  \item[ActorPluginState]
  definiert 4 Werte, welche das ActorPlugin annehmen kann. Diese 4 Werte sind SETUP, IDLE, MOVEMENT und ANIMATION.
  \item[FeedbackMessage]
  beschreibt die erste der beiden MessageQueue-Nachrichten, welche vom ActorPlugin an den ActorServer gesendet wird.
  In dieser Struktur befindet sich der aktuelle Plugin-Zustand als \code{state} Parameter vom Typ ActorPluginState.
  Außerdem ein \code{progress} Parameter in Form einer Gleitkommazahl, welche den Fortschritt der aktuellen Aktion angibt.
  Um bei Bewegungen die aktuelle Position des Menschen zu erhalten, ist auch die aktuelle Pose des Modells im Parameter \code{current} enthalten.
  \item[ActionMessage]
  ist die andere Nachricht, welche über die zweite MessageQueue vom ActorServer an das ActorPlugin gesendet wird.
  Wie in der FeedbackMessage ist ein \code{state} Parameter vom selben Typ enthalten, jedoch dient dieser hier als Vorgabe für den nächsten State.
  Ein \code{animationName} Parameter wird als ein char-Array mit einer maximalen Länge von 255 Zeichen übergeben.
  Dieser bestimmt später die Animation, welche je nach ActorPluginState während einer Bewegung oder Animation ausgeführt wird.
  Der \code{animationSpeed} Parameter beschreibt entweder die Abspielgeschwindigkeit der Animation, oder die Distanz, welche pro Animationsdurchlauf zurückgelegt wird.
  Außerdem wird im Falle einer Bewegung der Parameter \code{target} vom Typ Pose verwendet, um die Endposition und Rotation des Actors zu bestimmen.
\end{description}
\section{Welt}
Die Welt der Simulation wird im Paket \code{ign_world} zur Verfügung gestellt.
In diesem Paket sind sowohl die Geometrien der Welt, aber auch die benötigten Dateien zum Starten der Simulation enthalten.

In diesem Fall handelt es sich um den Raum, in welchem die Interaktion zwischen Mensch und Roboter stattfinden soll.
Für diesen Raum wurde zuerst ein Raumplan erstellt, welcher alle benötigten Bereiche für die Szenarien besitzt (Abbildung \ref{room-plan}).

Zuerst wird ein Stellplatz für den Roboter benötigt, welcher an einer Ecke des Raumes positioniert werden sollte.
Diese Position wurde gewählt, um die Verfahrgeschwindigkeit des Roboters höher zu halten, welche je nach Distanz zum Menschen angepasst werden soll.

Des weiteren werden eine Arbeits- und Lagersätte für den Menschen benötigt, welche in den Szenarien genutzt werden sollen.
Im Koexistenzszenario soll der Mensch nur an diesen Stellen seine Arbeit verrichten, jedoch sich selten dem Roboter nähern, um dessen Fortschritt zu begutachten.
Die Lagerstätte soll im Kooperationsszenario neben der Arbeit des Menschen auch für die fehlerhaften Teile verwendet werden, welche durch den Roboter aussortiert und durch den Menschen dorthin verbracht werden sollen.
Eine Nutzung im Kollaborationsszenario ist nicht nicht geplant, da der Mensch den Roboter überwachen und dessen Fehler korrigieren soll.

Der so geplante Raum wurde in Blender modelliert und als .stl-Datei exportiert, um sie in die Welt einbinden zu können.
Für das Kooperationsszenario wurde ein weiterer Raum mit Förderband erstellt, welcher nur dort genutzt wird.
Der so erstellte Raum wird nun in einer separaten .sdf-Datei referenziert, welche

Diese Entscheidung limitiert wie der Raum in die Simulation eingebunden wird, da mehrere unterschiedliche Räume je nach ausgewähltem Szenario geladen werden müssen.
Hierfür wird, wie später auch für den Roboter selbst, das Paket \code{ros_gz_sim} verwendet.
Dieses veranlasst mit dem \code{create}-Programm das Erstellen der übergebenen Datenstruktur in Gazebo.


\begin{figure}[hpt]
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Welt-Plan.drawio}
\centering
\caption{Geplanter Raum}
\label{roomplan}
\end{minipage}
\hspace{.1\textwidth}
\begin{minipage}{.45\textwidth}
\includegraphics[width=\textwidth]{img/MA-Umsetzung-Welt-Blender}
\centering
\caption{Umsetzung in Blender}
\label{roomfinished}
\end{minipage}
\end{figure}

\section{Mensch}
\subsection{Übersicht}
Das angepasste Verfahren zur Menschensteuerung in der Simulation verwendet mehrere Kommunikationswege.
Als erstes wird eine Bewegungs- oder Animationsanfrage an den ROS-Action-Server im ActorServer gesendet.
Wenn die Simulation aktuell keinen Befehl ausführt, wird diese Anfrage akzeptiert, ansonsten wird sie abgebrochen.
Daraufhin werden die Daten der Anfrage über eine Posix-Message-Queue vom ActorServer an das ActorPlugin in Gazebo gesendet.

Dieses verwendet die Daten, um eine interne State-Machine in den entsprechenden Zustand zu setzen, welcher zur Ausführung des Befehls benötigt wird.

Um Feedback an den Client des ROS-Action-Servers übertragen zu können, werden bei der Ausführung von Befehlen oder Zustandswechseln des ActorPlugins Feedbackdaten über eine separate MessageQueue zurück an den ActorServer übertragen.
Diese werden durch den ActorServer aufbereitet, da nicht alle Daten für die jeweilige laufende Aktion relevant sind und an den ROS-Action-Client gesendet.

Um diese Befehle in der Simulation auch visuell umsetzen zu können, werden weitere Animationen für das Modell des Menschen benötigt, welche im Kontext der zur erfüllenden Aufgabe relevant sind.
Dafür muss dan Modell in einen animierbaren Zustand gebracht werden, in welchem dann weitere Animationen erstellt und in die Simulation eingebunden werden können.
\subsection{Modellierung}
Um neue Animationen für den Menschen in der Simulation erstellen zu können, muss ein Modell für diesen erstellt werden.
Dafür wurde eine der inkludierten Animationen in Blender geöffnet und das visuelle Modell kopiert.

Dieses Modell war auf Grund von vielen inneren Falten nur schlecht für Animationen geeignet, weshalb das Modell an diesen Stellen vereinfacht wurde.
Eine solches Vorgehen beugt Anomalien bei der Animation durch unterschiedliche Verschiebung der Strukturen vor, welche vom inneren des Modells hervortreten können.

Nun musste das visuelle Modell mit neuen Animationsknochen versehen werden, da die in der Animation vorhandenen Knochen nicht verbunden sind.
Diese Knochen bilden ein Skelett, welches zur Animation bewegt werden kann.

In Blender können sogenannte Constraints verwendet werden, um die Gelenke in Gruppen zusammenzufassen und genauer zu spezifizieren.
Dazu wurde das Plugin ``Rigify'' verwendet, welches ein komplettes Skelett generiert und konfiguriert.

Dieses generierte Skelett kann nun an das Modell angepasst werden.
Um eine bessere Übersicht zu ermöglichen, sollten als erstes alle nicht benötigten Skeletteile, wie zum Beispiel für Gesichtsanimationen, entfernt werden.
Danach werden alle Knochen dem visuellen Modell angepasst.
Dabei muss auf die Ausrichtung der Knochen zueinander geachtet werden.
Das Kreuzprodukt der Vektoren beider Knochensegmente ist die Richtung der Beugeachse, welche sich im Verbindungspunkt beider Knochen befindet.
Ist diese nicht richtig ausgerichtet, wenn zum Beispiel beide Knochen auf einer Gerade liegen, verbiegen sich Gelenke bei der Verwendung von inverser Kinematik zur Positionsvorgabe falsch.

Das hier erstellte, verbesserte Rigify-Skelett kann nun einfacher animiert werden.
Dies liegt vor allem an neuen Markierungen in Blender, welche durch mehrere Constraints viele andere Knochen beeinflussen.
Beispielswise können Fuß- und Handmarkierungen gesetzt werden, welche die Rotation des Fußes oder der Hand, aber auch gleichzeitig die inverse Kinematik des gesamten Beins oder Arms automatisieren.
Selbiges gilt für neue Markierungen, welche zum Beispiel Hüft- und Kopfbewegungen vereinfachen.

Das Exportieren eines solchen Rigs ist jedoch schwierig, da viele Grafikengines keine verschachtelten Skelette verstehen.
Dies ist auch der Grund, warum die Skelette beim initialen Import mitgelieferter Animationen nicht verbunden waren.

Um aus einem existierenden, vollständig verbundenen Skelett einzelne Knochen zu extrahieren exisitiert ein weiteres Plugin mit dem Namen ``GameRig''.
Dieses separiert die Knochen wieder, um die Animationen für Grafikengines exportieren zu können.

Nach dieser Veränderung kann die Animation als Collada-Animation exportiert werden.
Dabei muss darauf geachtet werden, dass die vorwärts gerichtete Achse auf die richtige Achse gestellt ist.
Außerdem ist es später einfacher, wenn nur eine Animation in jeder Datei vorhanden ist.
Dies ist dem Fakt geschuldet, dass diese anderen Animationen zwar verfügbar sind, jedoch aber nur durch einen Index unterscheidbar sind, der von der Reihenfolge der exportierten Animationen abhängig ist.

\subsection{Programmierung}
\subsubsection{Message Queue}
Bei der Implementierung des ActorPlugins stellte sich heraus, dass die nun im ActorServer ausgelagerten Befehle mit den Befehlen im ros_control-Plugin kollidieren.
Dies geschieht, da beide Plugins rclcpp, eine Bibliothek zur Kommunikation mit ROS, verwenden.

In dieser Bibliothek wird eine globale Instanz angelegt, welche den Zustand des Kommunikationsprotokolls abbildet.
Da jedoch von beiden Plugins auf diesen Zustand zugegriffen wird, kommt es zur Problemen, da kein Synchronisationsmechanismus existiert.
Die dadurch entstehenden gleichzeitigen Zugriffe auf die selben Ressourcen führen zur Terminierung des Programms.

Eine Anpassung beider Plugins auf die gemeinsame Nutzung einer Ressource ist möglich, erfordert jedoch weitere Anpassungen, welche zeitlich nur schwer planbar sind.
Die Nutzung eines separaten Dienstes, welcher keinen globalen Kontext benötigt, ist die sicherste Lösung des Problems.
Durch einen solchen Dienst werden auch in Zukunft keine Plugins gestört, auch wenn sie selbigen Dienst zur Kommunikation verwenden.

Die Auswahl eines Dienstes wurde dabei aus einer Reihe an unterschielichen Möglichkeiten getroffen.
Eine REST-API hat den Vorteil, dass sie durch fast jede Programmiersprache genutzt werden kann, die Sockets unterstützt, hat jedoch keinen einheitlichen Feedbackmechanismus.
Die neueren Websockets bieten die Möglichkeit, bidirektional Daten zu übertragen und erlauben somit Feedback an das aufrufende Programm.
Beide Technologien basieren jedoch auf einem Webserver, welcher auf einem bestimmten Port des Systems ausgeführt werden muss, was Kollisionen mit anderen Serveices ermöglicht.
Die Portnummer kann zwar geändert werden, ist jedoch nicht einfach mit einer Komponente assoziierbar, was sie zu einer ``Magischen Zahl'' macht.
Dies sorgt für schlechte Lesbarkeit in einem wichtigen Teil des Kontrollflusses.
Außerdem besitzen beide Terchnologien durch TCP oder UDP und HTTP relativ großen Protokolloverhead, welcher bei den hohen Updateraten der Gazebo-Simulation zu Problemen führen könnte.

Eine andere Möglichkeit ist die Nutzung von ``shared memory'', einem geteilten Speicherbereich zwischen beiden Programmen.
Dieser kann zur bidirektionalen Kommunikation genutzt werden, da beide Programme auf den Speicher zugreifen können.
Alle Zugriffe auf den Bereich sind extrem schnell, was diese Technik ideal zur schnellen Datenübertragung zwischen Prozessen macht.
Durch das erlauben gleichzeitiger Zugriffe kann es hierbei vorkommen, dass die selbe Adresse gleichzeitig von einem Programm gelesen und von einem anderen geschrieben wird.
Die dabei gelesenen Daten können Schäden aufweisen, weswegen Zugriffe auf den Speicherbereich koordiniert werden müssen.

Die letzte betrachtete Methode ist die Verwendung einer Message Queue.
Hier wird im Betriebssystem ein Speicherbereich mit bestimmter Größe für den Datenaustauch reserviert.
Dieser Bereich besitzt ein Identifikationsmerkmal, mit welchem Anwendungen Zugriff auf diesen erlangen können.
Ein Programm kann in diesem Bereich Nachrichten ablegen, welche durch das andere Programm gelesen werden können.
Die Koordinierung der Zugriffe erfolgt dabei durch das Betriebssystem, was gleichzeitige Zugriffe, wie bei shared memory, aussschließt.
Hierdurch kommt es zu einem Anstieg an Latenzzeit, jedoch ist dieser ausreichend gering.

Die Wahl des Dienstes fiel auf eine MessageQueue, jedoch existieren unter Linux 2 unabhängige Implementationen.
Die erste Implementation ist die System V MessageQueue, und verwendet zur Identifikation einfache Integer.
Eine Spezialität dieser alten Implementation ist das Sortieren der Nachrichten nach Nachrichtentyp in der gleichen Warteschlange.
Die neuere Implementation der POSIX MessageQueue bietet einige weitere Funktionen, wie zum Beispiel aynchrone Benachrichtigungen bei neuen Nachrichten, Quality of Service und nutzt bis zu 256 Zeichen lange Strings zur Identifikation.

Die ausgewählte Implementation ist die neuere POSIX-Implementation einer Message Queue, da diese basierend auf den Erfahrungen mit der System V Implementation verbessert wurde.
\subsubsection{Nachrichten}
Die versendeten Nachrichten für den ActionServer, als auch für die Message Queue sind in den Paketen \code{ros_actor_action_server_msgs} und \code{ros_actor_message_queue_msgs} abgelegt.
Sie sind absichtlich nicht in den nutzenden Paketen untergebracht, da sie durch ein externes Programm in den entsprechenden Code umgewandelt werden.

Jede Action definiert 3 Nachrichten, welche zu unterschiedlichen Zeitpunkten in ihrer Ausführung verwendet werden.
Die definierten Nachrichten sind eine Startnachricht, eine Feedbacknachricht und eine Endnachricht.

Ein ActionServer definiert 3 Funktionen, welche die Handhabung einer Aktion definieren.
Die erste Funktion übergibt den Wert der Startnachricht, welche mit einer Antwort quittiert werden muss.
Hierbei sind die Antworten ACCEPT_AND_DEFER, ACCEPT_AND_EXECUTE und REJECT möglich.

ACCEPT_AND_EXECUTE signalisiert die sofortige Ausführung des gewünschten Befehls und ACCEPT_AND_DEFER steht für eine spätere Ausführung der gewünschten Aktion.
Die Option REJECT bricht die Ausführung der Aktion vorzeitig ab.

Die zweite Funktion übergibt Abbruchanfragen an den Server, falls die Aktion durch den Client abgebrochen werden soll.
Auch diese Anfrage kann entweder mit ACCEPT akzeptiert werden, oder mit REJECT zurückgewiesen werden.

Um Feedback während der Ausführung der Aktion geben zu können, exisitiert die dritte Funktion.
Diese erhält als Parameter ein Objekt, mit welchem Feedback- und Endnachrichten an den Client gesendet werden können.

In der Startnachricht werden alle Daten, welche der Server für die Bearbeitung einer Anfrage benötigt, übergeben.
Dies geschieht, damit der Auftrag schon beim Start abgebrochen werden kann, sollte dieser nicht erfüllbar sein.

Die Feedbacknachrichten werden vom Server an den Client zurück gesendet, solange das Programm ausgeführt wird.
Dabei ist es Aufgabe des Programms, diese in beliebigen Abständen an den Client zu senden.

Die Endnachricht kann Rückgabewerte für die ausgeführte Aufgabe enthalten, falls diese benötigt werden.
Sie werden in diesem Projekt nicht genutzt, da das Beenden eines Auftrags immer mit einer einfachen Erfolgs- oder Misserfolgsmeldung quittiert wird.

\subsubsection{ActorPlugin}
Das ActorPlugin nutzt die empfangenen Nachrichten aus der eingehenden Message Queue, um diese in der Simulation umzusetzen.
Der Code des Plugins ist dabei im Paket \code{ign_actor_plugin} organisiert, welches im Gazebo-Modell der Welt referenziert werden muss, um das Plugin zu laden.

Das Plugin erhält durch die empfangenen Nachrichten mehrere Zustände, welche im folgenden erläutert werden:
\begin{description}
 \item[Setup]
  wird ausschließlich zu Simulationsbeginn verwendet, um alle benötigten Referenzen aus der Simualtionumgebung im Plugin zu hinerlegen, so dass diese in den anderen Zuständen genutzt werden können.
 \item[Movement]
  bedeutet die Ausführung einer Bewegung in potentiell mehreren Schritten.
  Zuerst wird die Distanz zum Zielpunkt geprüft.
  Ist diese ausreichend gering, wird nur eine Bewegung in die gewünschte Endausrichtung durchgeführt.
  Ist diese größer, dreht sich der Actor in Richtung des Ziels und bewegt sich anschließend dorthin.
  Falls die gewünschte Endrotation nicht einem Null-Quaternion entspricht, wird anschließend noch eine Rotation in die Zielrichtung durchgeführt.
 \item[Animation]
  entspricht der Ausführung einer Animation an der aktuellen Position des Actors.
  Diese kann durch einen Skalierungsfaktor beschleunigt oder verlangsamt werden.
 \item[Idle]
  ist der Zustand, welcher nach erfolgreicher Ausführung eines Befehls angenommen wird.
\end{description}

Das ActorPlugin besitzt kein Konzept eines ROS-ActionServers und verlässt sich auf den ActorServer, welcher die Feedbacknachrichten in das richtige Format bringt.
Feedback wird in den Zuständen Movement und Animation in eine zweiten Message Queue zu jedem Simulationsschritt gesendet.
Um Zustandsübergänge erkennen zu können, werden auch diese als Feedback an den ActorServer gesendet.
\subsubsection{ActorServer}
Der ActorServer ist die Brücke zwischen ROS und dem ActorPlugin und ist im Paket \code{ros_actor_action_server} enthalten.
Dieser weitere Dienst bindet das ActorPlugin an ROS an.

Es werden dafür zwei ROS-ActionServer gestartet, welche jeweils Bewegungen oder Animationen des simulierten Menschen auslösen können.
Beide ActionServer prüfen bei dem Empfang eines neuen Ziels zuerst, ob bereits eine andere Aktion ausgeführt wird.
Sollte bereits eine andere Aktion ausgeführt werden, wird die Anfrage abgelehnt.
Im anderen Fall wird die Aufgabe akzeptiert und in das MessageQueue-Format übersetzt und an das ActorPlugin gesandt.

Um das Starten mehrerer gleichzeitiger Aktionen zu unterbinden, muss der Empfang einer neuen Anfrage bestätigt werden, bevor weitere Befehle über den ROS-ActionServer entgegen genommen werden können.
Hierzu wird ein Mutex verwendet, welcher die Auswertung neuer Nachrichten verhindert, so lange der aktuelle Befehl noch nicht durch das Plugin bestätigt wurde.
Parallel werden alle eingehenden Feedback-Nachrichten der Message Queue des ActorPlugins in Feedback für die aktuell laufende Action umgewandelt.

Im Falle des Bewegungs-ActionServers werden mehrere Parameter benötigt.
Zuerst werden Animationsname und -diztanz benötigt, um die richtige Animation auszuwählen und die Bewegung mit der Animation zu synchronisieren.
Als Feedbacknachricht erhält der Client die aktuelle Pose des Actors im Simulationsraum.

Soll eine Animation über den Action Server abgespielt werden, wird auch hier ein Animationsname, jedoch auch eine Animationsgeschwindigkeit benötigt.
Die Feedbacknachricht enthält den Fortschritt der Animation als Gleitkommazahl.
\section{Roboter}
\subsection{Übersicht}
Der Roboter besteht aus vielen interagierenden Systemen, welche in ihrer Gesamtheit das vollständige Robotermodell in der Simulation verwendbar machen.

Zuerst muss ein Modell des Roboters erstellt werden, welches in Gazebo geladen werden kann.
Dieses Modell muss dann für die Bewegungsplanung mit MoveIt erweitert werden.
Hierbei werden Controller von ros_control mit dem Modell verbunden, um den aktuellen Zustand der Achsen zu überwachen und diese steuern zu können.

Um diese Vielfalt an Daten standartisiert an andere Software in ROS weitergeben zu können, wird eine MoveGroup in ROS gestartet, welche die Planung von Bewegungen durch andere Programme zulässt.
\subsection{Modellierung}
Für den Kuka LBR iisy existiert kein Simulationsmodell für Gazebo und ROS, weswegen dieses Modell aus Herstellerdaten generiert wurden.
Hierzu stand eine .stl-Datei des Herstellers zur Verfügung.
Aus dieser Datei wurden mit FreeCAD\cite{freecad} alle Glieder des Roboters als Collada-Dateien exportiert.
Dabei muss darauf geachtet werden, dass die exportierten Daten eine ausreichende Meshgröße haben, welche vorher in FreeCAD eingestellt werden muss.
Dies erlaubt das Erhalten der feinen Strukturen des Arms, auch visualisiert in Abbildung \ref{robot_raw}, welche erst später in Blender reduziert werden sollen.

\begin{figure}[]
\includegraphics[width=\textwidth/2]{img/MA-Roboter-Rohdaten}
\centering
\caption{Rohdaten aus .stl-Datei}
\label{robot_raw}
\end{figure}

Diese Dateien können dann in Blender bearbeitet werden, um sie für die Simulation tauglich zu machen.
Hierfür wurde die hohe Auflösung der Modelle reduziert, was sich in kleineren Dateien und Startzeiten der Simulation, aber auch in der Renderzeit der Simulation, auswirkt.
Außerdem wuden die Glieder so ausgerichtet, dass der Verbindungspunkt zum vorherigen Glied im Nullpunkt des Koordinatensystems befindet.
Das vollständige visuelle Modell ist in Abbildung \ref{robot_visual} zu sehen.

Um die Simulation weiter zu beschleunigen, wurden die Kollisionsboxen des Arms noch weiter vereinfacht, was die Kollisionsüberprüfung dramatisch beschleunigt.
Dabei werden stark simplifizierte Formen verwendet, welche das hochqualitative visuelle Modell mit einfachen Formen umfassen.
Das resultierende Modell, welches in Abbildung \ref{robot_collision} dargestellt wird, wird später zur Kollisionserkennung verwendet.

Diese Herangehensweise ist nötig, da Kollisionserkennung auf der CPU durchgeführt wird, welche durch komplexe Formen stark verlangsamt wird.

Um aus den generierten Gliedermodellen ein komplettes Robotermodell erstellen zu können, wurde eine .urdf-Datei erstellt.
In dieser werden die verwendeten Gelenktypen zwischen den einzelnen Gliedern, aber auch deren Masse, maximale Geschwindigkeit, maximale Motorkraft, Reibung und Dämpfung hinterlegt.
Diese Daten können später zur Simulation der Motoren genutzt werden, welche den Arm bewegen.

Die Gelenkpositionen sind dabei relative Angaben, welche sich auf das Glied beziehen, an welchem ein weiteres Glied über das Gelenk verbunden werden soll.
Alle kontrollierbaren Gelenke benötigen auch eine Gelenkachse, welche je nach Gelenktyp die mögliche Beweglichkeit bestimmt.

Alle hier erstellten Dateien wurden im Paket \code{iisy_config} zusammengefasst, um diese einfacher wiederauffinden zu können.
\begin{figure}[hpt]
\begin{minipage}{.5\textwidth}
\includegraphics[width=\textwidth]{img/MA-Roboter-Visuell}
\centering
\caption{Visuelles Modell}
\label{robot_visual}
\end{minipage}
\begin{minipage}{.5\textwidth}
\includegraphics[width=\textwidth]{img/MA-Roboter-Kollision}
\centering
\caption{Kollisionsmodell}
\label{robot_collision}
\end{minipage}
\end{figure}
\subsection{MoveIt 2 Konfiguration}
Das somit erstellte Paket kann nun mit dem neu implementierten MoveIt Configurator um die benötigten Kontrollstrukturen erweitert werden.
Dazu wurde der neue Setupassistent von MoveIt2 verwendet, welcher das Modell analysiert, um es mit für MoveIt benötigten Parameter zu erweitert.

Die Erstellung des erweiterten Modells mit dem Assistenten funktionierte komplett fehlerfrei, jedoch ließen sich die generierten Dateien nicht nutzen.

Um die generierten Dateien nutzen zu können, mussten diese weiter angepasst werden.
Dies beinhaltete die korrekte Integration der Roboterdefinitionen im Paket, aber auch zahlreiche Pfadreferenzen auf verwendete Dateien, welche nicht korrekt generiert wurden.
Diese können standartmäßig nicht in Gazebo, RViz und MoveGroup gleichzeitig geladen werden, da diese Programme unterschiedliche Pfadangaben verlangen, was die Nutzung verhindert.

Eine Möglichkeit zur Lösung des Problems ist die Verwendung von \code{file://}-URIs, welche von allen dieser Programme gelesen werden können.
Jedoch ist hier als Nachteil zu verzeichnen, dass der Moveit Setup Assistent diese URIs nicht lesen kann, was zu Fehlern bei einer Rekonfiguration führen kann.

Das so erstellte Modell kann ber den Aufruf von \code{ros2 launch iisy_config demo.launch.py} in RViz getestet werden.
Hierfür erscheint das Robotermodell mit mehreren Markern und Planungsoptionen, welche genutzt werden können, um beliebige Bewegungen zu planen und auszuführen.
\subsection{Details}
Das so erstellte Modell kann nun zur Laufzeit in Gazebo geladen werden.
Dafür wird das Paket \code{ros_gz_sim} verwendet, welches das \code{create} Programm beinhaltet.
Mit diesem Werkzeuk kann ein Modell unter einem bestimmten Namen anhand einer Datei oder eines übergebenen Strings in Gazebo importiert werden.
Das Modell kann dabei über Argumente im Raum verschoben und rotiert werden, falls diese Funktionalität benötigt wird.

Im Modell sind auch die verwendeten Gazebo-Plugins deklariert, welche für die Integration mit \code{ros_control} verantwortlich sind.
Das Gazebo-Plugin läd dabei die verwendeten Controller und versorgt diese mit Informationen über den Roboter.

Die zusätzlichen Programme werden nun in die gazebo_controller.launch.py-Datei

\section{Behavior Trees}
Alle Behavior Trees wurden im Paket \code{btree} organisert, welches die Bäume im XML-Format und die Implementation der Nodes und umliegenden Infrastruktur enthält.


Für die Umsetzung des Szenarios wurden neue Nodes für den BehaviorTree erstellt.
Diese lassen sich nach Nutzung in verschiedene Gruppen einordnen.

\subsubsection{Allgemein nutzbare Nodes}
\begin{description}
  \item[GenerateXYPose]
  generiert eine Pose in einem durch den \code{area} Parameter angegebenen Bereich.
  Um dies zu ermöglichen, wird zuerst die Fläche aller Dreiecke berechnet, welche den Bereich definieren.
  Diese werden durch den Gesamtinhalt geteilt, um eine Wichtung der Dreiecke zum Gesamtinhalt zu erreichen.
  Nun wird eine Zufallszahl zwischen 0 und 1 gebildet.
  Von dieser werden nun die Wichtungen der Dreiecke abgezogen, bis dies Zufallszahl im nächsten abzuziehenden Dreieck liegt.
  Nun wird in diesem Dreieck eine zufällige Position ermittelt, welche ber den Ausgabeparameter \code{pose} ausgegeben wird.
  \item[InAreaTest]
  prüft, ob eine Pose, vorgegeben durch den \code{pose} Parameter, in einer durch den \code{area} Parameter definierten Zone liegt.
  Hierfür wird überprüft, ob die X und Y-Werte der Pose in einem der Dreiecke liegen, welche die Area definieren.
  Der Rückgabewert ist das Ergebnis dieser Überprüfung, wobei SUCCESS bedeutet, dass sich die Pose in der Area befindet.
  \item[OffsetPose]
  wird genuzt, um eine Pose im Raum zu bewegen und/oder deren Orientierung zu verändern.
  Falls der \code{offset} Parameter gesetzt ist, wird dieser mit dem \code{input} Parameter summiert.
  Außerdem wird die Orientierung der Pose auf den \code{orientation} Parameter gesetzt, falls dieser vorhanden ist, was den ursprünglichen Wert überschreibt.
  \item[InterruptableSequence]
  stellt eine Sequence dar, welche auch nach ihrem Abbruch ihre Position behält.
  Dies ist von Nöten, wenn bestimmtes Verhalten unterbrechbar ist, aber zu einem späteren Zeitpunkt fortgesetzt werden soll.
  Hierzu wird der Iterator der unterlegenden Sequenz nur erhöht, wenn die untergeordnete Node SUCCESS zurück gibt.
  Außerdem wird der Iterator nicht zurückgesetzt, wenn die Ausführung dieser modifizierten Sequenz abgebrochen wird.
  \item[WeightedRandom]
  ist eine Steuerungsnode, welche mehrere untergeordnete Nodes besitzt.
  Dabei werden diese nicht, wie bei anderen Steuerungsnodes üblich, sequentiell ausgeführt.
  Anhand einer vorgegebenen Wichtung im \code{weights} Parameter wird eine der untergeordneten Nodes zufällig ausgewählt.
  Diese Node wird nun als einzige Node ausgeführt, bis diese den SUCCESS-Status zurück gibt.
  Nach dem dieser Status erreicht wurde, wird bei dem nächsten Durchlauf eine neue Node ausgewählt.
  Der Rückgabewert ist der Rückgabewert der ausgewählten untergeorneten Node.
  \item[IsCalled]
  fragt den aktuellen Called-Status des Actors ab, welcher in einigen Szenarien vom Roboter verwendet wird, um den simulierten Menschen zu rufen.
  Der Rückgabewert der Node ist dabei SUCCESS, falls der Mensch gerufen wird, oder FAILURE, wenn kein RUF durchgeführt wird.
  \item[SetCalledTo]
  setzt den aktuellen Called-Status auf den Wert des übergebenen \code{state} Parameters.
  Da diese Aktion nicht fehlschlagen kann, liefert diese Node immer SUCCESS als Rückgabewert.
  \item[RandomFailure]
  generiert eine Zufallszahl von 0 bis 1, welche mit dem \code{failure_chance} Parameter verglichen wird.
  Der Rückgabewert ist das Ergebnis des Vergleichs, FAILURE, wenn die Zufallszahl kleiner als der \code{failure_chance} Parameter ist, oder im anderen Falle SUCCESS.
\end{description}

\subsubsection{Menschenspezifisch}
\begin{description}
  \item[ActorAnimation]
  wird verwendet, um dem simulierten Menschen eine auszuführende Animation zu senden.
  Die Node benötigt zur Ausführung einen Animationsname, welcher im \code{animation_name} Parameter angegeben wird.
  Ein optionaler \code{animation_speed} Parameter gibt die Ausführungsgeschwindigkeit vor.
  Der Rückgabewert ist SUCCESS, wenn die komplette Ausführung gelang, oder FAILURE, falls diese abgebrochen oder abgelehnt wurde.
  \item[ActorMovement]
  funktioniert wie eine ActorAnimation, sendet jedoch eine Bewegungsanfrage.
  Auch für diese wird ein Animationsname benötigt, welcher im \code{animation_name} Parameter definiert wird.
  Jedoch wird für die Synchronisation zur Bewegung ein Diztanzwert bewnötigt, welcher in einem Animationsdurchlauf zurückgelegt wied.
  Dieser wird im \code{animation_distance} Parameter übergeben.
  Eine Zielpose wird im \code{target} Parameter gesetzt.
  Eine Besonderheit dieses Paramerters ist die Verwendung des Null-Quaternions als Richtungsangabe, was die Endrotation auf die Laufrichtung setzt.
\end{description}

\subsubsection{Roboterspezifisch}
\begin{description}
  \item[RobotMove] gibt dem Roboter eine neue Zielposition über den \code{target} Parameter vor.
  Bei dieser Node handelt es sich um eine asynchrone Node, welche nur bei der erfolgreiche Ausführung der Bewegung SUCCESS zurück gibt.
  \item[SetRobotVelocity] setzt eine neue maximale Geschwindigkeit des Roboters, vorgegeben durch den \code{velocity} Parameter.
  Der Rückgabewert ist immer SUCCESS.
\end{description}
Diese beiden roboterspezifischen Nodes beeinflussen im Hintergrund das gleiche System, da Bewegungen bei Geschwindigkeitsänderungen neu geplant und ausgeführt werden müssen.
Dazu wird das letzte Ziel bis zu dessen erreichen vorgehalten, um die Ausführung neu zu starten, fall eine Geschwindigkeitsänderung durchgeführt werden muss.
Um die RobotMove-Node nicht zu unterbrechen, wird diese nur über einen Callback über den Erfolg oder Misserfolg der gesamten Bewegung und nicht über den Erfolg einzelner Teilbewegungen informiert.

\section{Docker-Compose}
Um Docker für die Verwaltung einer ROS-Installation verwenden zu können, müssen einige Anpassungen vorgenommen werden.
Da viele Anwendungen, unter anderem auch die Simulationsumgebung, eine Desktopumgebung benötigen, muss eine Zugriffsmöglichkeit geschaffen werden.

Diese Möglichkeiten können nicht durch Docker allein gelöst werden, da Befehle auf dem Hostsystem ausgeführt werden müssen, um die gewünschten Funktionen zu aktivieren.
Um diese Modifikiationen trotzdem reproduzierbar zu machen, wurde ein Shellscript geschrieben, welches zum Starten des Containers verwendet wird.

Dieses Skript erstellt zuerst die benötigten Verzeichnisse für den Container, falls diese noch nicht existieren.
Danach werden die SSH-Keys des Hosts in den Container kopiert, um eine SSH-Verbindung zu ermöglichen.
Dieser Umweg über SSH ist nötig, da die benötigten Umgebungsvariablen für ROS sonst nicht in allen Fällen gesetzt werden können.

Außerdem werden die benötigten Zugriffe auf den lokalen X-Server durch den Container anhand dessen Hostname erlaubt.
Diese Änderung erlaubt es dem Container, Fenster auf dem Desktop anzeigen zu können, solange die benötigten SysFS-Dateien hereingereicht werden.
Dies geschieht durch Einträge in der compose.yml-Datei, welche diese als ``bind mount'' in den Container hereinreicht.

Um Zugriff auf die Grafikbeschleunigung des Systems zu erhalten, muss deren Repräsentation im SysFS unter \code{/dev/dri} hineingereicht werden.
Der Zugriff auf die Desktopumgebung, welcher bereits entsperrt wurde, wird nun durch das mounten von \code{/tmp/.X11-unix} erreicht.
Dabei handelt es sich um den Unix-Socket des X11 Displayservers.

Zum Starten des Containers muss der Befehl \code{./start.sh} im Verzeichnis der Containerinstallation ausgeführt werden, welcher die obengenannten Schritte ausführt und den Container startet.
Eine Verbindung zum Container kann aufgebaut werden, nachdem die Meldung \code{ros_1 | Ready to connect.} in der Konsole erscheint.

Dafür muss ein SSH-Client eingesetzt werden, welcher eine Verbindung zu der lokalen Netzadresse des Systems mit dem Benutzer \code{ros} aufbauen kann.
Der Port des SSH-Servers wird dabei durch die Deklaration im docker-compose.yaml an das Hostsystem durchgereicht.
Hierbei ist zu beachten, dass der SSH-Server im Container auf Port 2222 ausgeführt wird, was bei der Verbindung beachtet werden muss.

Nach der Verbindung wird automatisch die ROS2-Umgebung eingerichtet, welche sofort nach Verbindungsaufbau genutzt werden kann.
Um die erstellten Pakete zu kompillieren, kann das Skript \code{build.sh} im \code{workspace}-Verzeichnis verwendet werden.

Dieses Skript nutzt \code{colcon} um alle Pakete in diesem Workspace zu erstellen.
Dabei wird auch noch eine \code{compile_commands.json}-Datei erstellt, welche von einigen IDE's zur Syntaxvervollständigung genutzt werden.
Außerdem wird der Event-Handler \code{console_cohesion+} verwendet, welcher das Lesen der Kompillierausgabe erleichtert.