@@ -146,26 +146,45 @@ Die Drucksensoren veröffentlichen ihren Zustand auf einem Topic, welches von de
\textcolor{blue}{Kurze Beschreibung, wie die im Entwurf beschriebene Architektur unter Nutzung von ROS implementiert wurde und wie die Constraints in MoveIt umgesetzt worden sind. + Schwierigkeiten}
Implementiert wurde die Demo in \textit{C++} und der ROS Distribution ROS Melodic Morenia auf einem Ubuntu 18.04 System.
Implementiert wurde die Demo in \textit{C++} und der ROS Distribution ROS Melodic Morenia auf einem Ubuntu 18.04 System. Zur Kapselung von Verantwortlichkeiten und für eine bessere Wiederverwendbarkeit, ist die Implementation in mehrere Packages aufgeteilt, die über Cmake miteinander gelinkt werden. In den folgenden Abschnitten wird auf die zentralen Packages einzeln eingegangen.
\subsection{Cobot}
Die Pakete \textit{cobot\_1} und \textit{cobot\_2} enthalten jeweils die Implementationen des \textit{CobotController} und der \textit{Cobot}-Klasse. Der \textit{CobotController} ist nicht als tatsächliche Klasse im Sinne der Objektorientierung implementiert, sondern als ROS Node. Das bedeutet er enthält eine Main Methode, die als Einstiegspunkt gilt, sobald er durch den ROS Master initialisiert wird.
Nach erfolgreicher Initialisierung, wird das Topic \glqq pressure\_1\grqq{} beziehungsweise \glqq pressure\_2\grqq{} abonniert, um informiert zu werden, sobald das Glas auf den Drucksensor abgestellt wird. Ein Service Client wird zum Stellen von Anfragen an den \textit{SafezoneController} erstellt und mit Hilfe des \textit{ObjectCreators} werden die Umgebungsobjekte, wie der Tisch, die Behälter und die Drucksensoren, zur Planning Scene hinzugefügt. Erst anschließend beginnt die Abarbeitung des Ablaufdiagramms~\ref{fig:ablaufdiagramm}. Die einzelnen Zustände beziehungsweise Aufgaben werden von einer \textit{Cobot}-Instanz ausgeführt, die für jede Aufgabe eine entsprechende Methode bereitstellt.
Nach erfolgreicher Initialisierung, wird das Topic \glqq pressure\_1\grqq{} beziehungsweise \glqq pressure\_2\grqq{} abonniert, um informiert zu werden, sobald das Glas auf den Drucksensor abgestellt wird. Ein Service Client wird zum Stellen von Anfragen an den \textit{SafezoneController} erstellt und mit Hilfe des \textit{ObjectCreators} werden die Umgebungsobjekte, wie der Tisch, die Behälter und die Drucksensoren, zur Planning Scene hinzugefügt. Erst anschließend beginnt die Abarbeitung des Ablaufdiagramms~\ref{fig:ablaufdiagramm}. Die einzelnen Zustände beziehungsweise Aufgaben werden von einer \textit{Cobot}-Instanz ausgeführt, die für jede Aufgabe eine entsprechende Methode bereitstellt. Eine Implementation einer solchen Aufgabe ist im Codebeispiel~\ref{lst:pick_bottle} dargestellt.
Neben dem
\lstinputlisting[firstline=27,lastline=39, label={lst:pick_bottle}, caption={Beispielimplementation einer Cobot-Aufgabe}]{../cobot_ws/src/cobot_1/src/Cobot.cpp}
- Config
- Launch files
Alle Aufgaben erwarten eine Referenz zu einer Move Group, auf der die Bewegung ausgeführt werden soll. Beinhaltet die Aufgabe ein Objekt, wie die Flasche beim Aufnehmen der Flasche, wird zusätzlich noch eine \textit{CollisionObject}-Beschreibung dieses Objektes benötigt, da in ihr Größe und Position des Objektes definiert ist. Die tatsächliche Implementation, zum Aufnehmen und Platzieren von Objekten, ist in einem weiteren Package ausgelagert, damit beide Cobots auf diese Funktionalität zugreifen können und Codeduplikate vermieden werden. Nach Ausführung der Bewegung, werden die notwendigen Constraints angepasst und dem \textit{CobotController}, in Form eines booleschen Werts, den Erfolg der Ausführung zurückgegeben.
Neben dem Programmcode enthält das Cobot Package auch noch eine Konfigurationsdatei, in der die vorhandenen Objekte und die Eigenschaften des Sensors beschrieben sind. Dadurch lässt sich die Ausgangssituation anpassen, ohne dass Änderungen am Programmcode notwendig sind, was die Nutzerfreundlichkeit erheblich steigert. Beim Start des Nodes werden die Parameter auf den Parameter Server veröffentlicht, wodurch sie auch von jedem anderen Node abgerufen werden können. Die Position der Flasche auf der X-Achse wird zum Beispiel durch den Parameter \verb|/object/bottle/x: 0.5| konfiguriert. Die Namensgebung der Parameter folgt dabei der ROS-Namenskonvention, um eine Überschneidung auf dem Parameter Server zu vermeiden.
Über die Launch Datei des Packages startet neben dem \textit{CobotController} auch den \textit{SafezoneController}- und \textit{PressureSensor}-Node und eine Simulationsumgebung.
\subsection{Constraints}
Das Package \textit{constraints} enthält die Definition der abstrakten \textit{Constraint}-Klasse und die \textcolor{blue}{fünf} konkreten Implementationen \textit{AccelerationConstraint}, \textit{OrientationConstraint}, \textcolor{blue}{\textit{ProximityConstraint}}, \textit{SafezoneConstraint} und \textit{VelocityConstraint}.
Der Orientierungs-Constraint (Codebeispiel~\ref{lst:orientation_constraint}) wird realisiert, indem eine \verb|moveit_msgs::OrientationConstraint| Nachricht erstellt wird, die die aktuelle Orientierung des Endeffektors festsetzt. Diese Nachricht wird dann der Move Group als Pfad-Constraint hinzugefügt. Entfernt wird das Constraint durch das Leeren der Liste \verb|path_constraints.orientation_constraints|.
\lstinputlisting[firstline=11,lastline=26, label={lst:orientation_constraint}, caption={Implementation des Orientierungs-Constraint}]{../cobot_ws/src/constraints/src/OrientationConstraint.cpp}
Die Beschleunigung und Geschwindigkeit wird beschränkt und wieder freigegeben, indem die Geschwindigkeits- beziehungsweise Beschleunigungsskalierung der Move Group angepasst wird.
Die Sicherheitszone, um den zweiten Drucksensor herum, wird durch das Hinzufügen eines weiteren Objekts zur Planning Scene realisiert, ohne dass dieses auch in der Simulationsumgebung erzeugt wird (siehe Codebeispiel~\ref{lst:safezone_constraint}). Das Erstellen des Kollisionsobjekts geschieht über den \textit{ObjectCreator} (Abschnitt~\ref{ch:objectCreator}). Darf der Roboter die Sicherheitszone betreten, wird das Safezone-Objekt entfernt, indem ein weiteres Objekt mit der selben ID und der \verb|REMOVE| Operation auf die Planning Scene angewandt wird.
\lstinputlisting[firstline=12,lastline=26, label={lst:safezone_constraint}, caption={Implementation des Safezone-Constraint}]{../cobot_ws/src/constraints/src/SafeZoneConstraint.cpp}
\subsection{SafezoneController}
Der \textit{SafezoneController}-Node stellt einen einfachen ROS Service unter dem Namen \verb|safe_zone_controller| zur Verfügung. Der Zustand der Sicherheitszone (frei/belegt) wird in einer globalen Variable gespeichert. Jede Anfrage an den Service wird von der Callback Funktion in Codebeispiel~\ref{lst:safezone_controller} verarbeitet. Wenn die Sicherheitszone frei ist, sind Anfragen immer erfolgreich, unabhängig davon, ob sie mit \verb|req.occupy == false| erneut freigegeben oder mit \verb|req.occupy == true| belegt werden soll. Ist die Zone bereits belegt, schlägt eine weitere Belegungsanfrage fehl, während das Freigeben der belegten Zone erfolgreich ist. Auf eine Überprüfung des Senders, dass nur der Akteur, der aktuell die Zone belegt, sie auch wieder freigeben kann, wurde an dieser Stelle verzichtet. \textcolor{red}{Ist das bei ROS Services überhaupt möglich?}
\subsection{ObjectCreator}
\lstinputlisting[firstline=17,lastline=32, label={lst:safezone_controller}, caption={Callback Funktion des SafezoneController}]{../cobot_ws/src/safe_zone_controller/src/SafeZoneController.cpp}
Das package \textit{object\_creator} enthält die Klassen \textit{ObjectCreator} und \textit{ObjectConfig}. Mit Hilfe der Klasse \textit{ObjectConfig} wird ein Konfigurationsobjekt aus den Konfigurationsparametern erstellt, die über den ROS Parameter Server abgerufen werden. Dieses Objekt enthält anschließend die Maße, Position, Masse und ein Flag, welches festlegt, ob es in der Simulation statisch oder bewegbar sein soll.
Anhand dieses Objekts kann der \textit{ObjectCreator} entsprechende Kollisionsobjekte erstellen und diese zur Planning Scene hinzufügen. Optional kann das erstellte Objekt auch zur Simulationsumgebung hinzugefügt werden.
\subsection{PressureSensor}
Als Drucksensor wird die Tinkerforge Load\_Cell\_V2\footnote{\url{https://www.tinkerforge.com/de/doc/Hardware/Bricklets/Load_Cell_V2.html}} verwendet. Für den Sensor steht ein \textit{C/C++} API Binding zur Verfügung, welches mit im \textit{pressure\_sensor} Package liegt und vom \textit{PressureSensor}-Node eingebunden wird. Dieser inseriert das \verb|pressure| Topic beim ROS Master und lädt die Konfigurationsparameter des Drucksensors vom ROS Parameter Server. Die Parameter enthalten sowohl die Adresse des Sensors als auch den Grenzwert in Gramm, ab dem das Glas als vorhanden angesehen werden soll. Bei jeder Änderung in der Messung, wird das Gewicht neu ausgewertet und der Zustand auf dem Topic veröffentlicht.
