implementierung.tex

\newpage\null\newpage
\chapter{Fallstudie}\label{ch:implementation}
Die Fallstudie soll die Anwendbarkeit der in der Taxonomie beschriebenen Constraints zeigen und untersuchen, in wie fern diese Constraints vom Motion Planning Framework MoveIt\footnote{\url{https://moveit.ros.org/}} unterstützt werden. Dazu wird eine Auswahl an Constraints in einem kollaborativen Anwendungsfall implementieren und untersuchen. Die Ausgangssituation bilden zwei Panda Roboterarme des Herstellers Franka Emika\footnote{https://www.franka.de/}. Der erste Roboter nimmt nach Initialisierung durch einen Menschen ein Gefäß auf und füllt dessen Inhalt in ein anderes Gefäß. Die Initialisierung erfolgt, indem ein leerer Behälter auf einem Drucksensor abgestellt wird. Nachdem das erste Gefäß wieder abgestellt wurde, wird das zweite Gefäß aufgenommen und auf einem zweiten Drucksensor in der Nähe des anderen Roboters gestellt. Dieser nimmt das Gefäß auf und stellt es dem menschlichen Nutzer bereit.

\section{Anforderungen}\label{ch:requirements}
Die in der Aufgabenstellung beschriebenen Handlungen der Roboter ergeben folgende Anforderungen:

\begin{enumerate}
	\item[H1] Ein Drucksensor soll das Vorhandensein eines Gefäßes signalisieren
	\item[H2] Beide Roboter sollen in der Lage sein die Gefäße aufzunehmen
	\item[H3] Beide Roboter sollen das gefüllte Gefäß bewegen können, ohne dessen Inhalt zu verschütten
	\item[H4] Der erste Roboter soll den Inhalt des einen Gefäßes in das andere umfüllen
	\item[H5] Es darf zu keinen Kollisionen zwischen den Robotern kommen
	
	
\end{enumerate}
Um einen reibungslosen Ablauf zu gewährleisten und den gestellten Anforderungen gerecht zu werden, sind folgende Constraints zu implementieren:

\begin{enumerate}
	\item[C1] Orientierung des Endeffektors: Während der Handhabung von gefüllten Gefäßen, sollen diese stets orthogonal zum Boden orientiert sein
	
	\item[C2] Beschleunigung und Geschwindigkeit: Gefüllte Gefäße müssen vorsichtig bewegt werden, um ein Überschwappen zu verhindern
	
	\item[C3] Arbeitsbereich: Um eine Kollision der beiden Roboter zu vermeiden, soll um die Übergabestelle eine Sicherheitszone eingerichtet werde, die immer nur von einem Roboter geschnitten werden darf
	
	\item [C4] Handlung: Die Handlungen dürfen nur in der durch die Aufgabenstellung vorgeschriebenen Reihenfolge durchgeführt werden
\end{enumerate}
Zusätzlich sind noch weitere handlungsunabhängige Anforderungen zu berücksichtigen:
\begin{enumerate}
	\item[A1] Constraints sollen aufgabenspezifisch angewandt und entfernt werden können
	\item[A2] Für eine höhere Usability und einer einfacheren Integration in andere Projekte, soll die Größe und Position der Objekte über eine Konfigurationsdatei anpassbar sein
	\item[A3] Zur Implementierung soll das Robot Operating System (ROS) und das Motion Planning Framework MoveIt verwendet werden
\end{enumerate}

\section{Entwurf}
In diesem Abschnitt wird, nach einer kurzen Einführung in ROS und MoveIt ein Entwurf vorgestellt, wie die gestellten Anforderungen aus Abschnitt~\ref{ch:requirements} technisch umgesetzt werden können.

\subsection{Robot Operating System}
Das Robot Operating System (ROS) ist ein mehrsprachiges open-source Framework zur flexiblen Realisierung komplexer Robotikanwendungen~\cite{quigley_ros_nodate}. In dieser Arbeit verwendet wird die Distribution \glqq ROS Melodic Morenia\grqq{}. Die Grundlage einer ROS Anwendung bilden sogenannte Nodes, die in einer Peer-to-Peer Architektur miteinander kommunizieren können. Im Folgenden werden die grundlegenden Begriffe kurz erläutert.

\paragraph{Node}
Ein Node ist ein eigenständiges Softwaremodul und ein eigenständiger Prozess, der parallel zu anderen Nodes ausgeführt werden kann und verschiedenste Berechnungen und Befehle ausführt. Ein ROS-basiertes System sollte in der Regel möglichst feingranular aufgebaut und Funktionalitäten in einzelne Nodes gekapselt sein. Ein vollständiges System besteht dementsprechend aus einer Menge an Nodes, die über Messages und Services miteinander kommunizieren. Dies erlaubt eine klare Trennung von Verantwortlichkeiten innerhalb des Systems und reduziert die Code-Komplexität, da zur Ansteuerung anderer Nodes keine Implementierungsdetails bekannt sein müssen.


\paragraph{Message}
Eine Message wird in einer Textdatei definiert und beschreibt eine streng typisierte Datenstruktur. In ihr können sowohl primitive Datentypen als auch Arrays von primitiven Datentypen und auch anderen Messages verwendet werden. Dadurch können sich beliebig tiefe Datenstrukturen aufbauen. Als Beispiel ist in Quelltext~\ref{lst:msg_example} die Definition der von MoveIt bereitgestellten Orientierungs-Constraint Message dargestellt. Sie enthält einfache Datentypen und weitere Messages.

\newpage
\begin{lstlisting}[language=C++, caption=Beispieldefinition einer Orientierungs-Constraint Message, label=lst:msg_example]
	std_msgs/Header header
	geometry_msgs/Quaternion orientation
	string link_name
	float64 absolute_x_axis_tolerance
	float64 absolute_y_axis_tolerance
	float64 absolute_z_axis_tolerance
	float64 weight
	
\end{lstlisting}


\paragraph{Topic}
Um eine Message zu senden, veröffentlicht ein Node diese Nachricht auf einem Topic. Definiert ist ein Topic durch einen namensgebenden String, wie zum Beispiel \glqq odometry\grqq{}. Interessiert sich ein Node für die veröffentlichten Informationen, kann er dieses Topic abonnieren und erhält dadurch stets die aktuellsten Daten, sobald diese veröffentlicht wurden.
Jeder Node kann sowohl mehrere Topics abonnieren als auch auf mehreren Topics seine Nachrichten veröffentlichen. Ebenso können mehrere Nodes auf dem selben Topic veröffentlichen.
Da Nodes in der Regel nichts über die Existenz der anderen Nodes wissen, werden alle Topics auf dem ROS Master inseriert, bei dem sich jeder Node über die verfügbaren Topics informieren kann. Wurde ein Topic abonniert, erfolgt der Datenaustausch allerdings direkt zwischen den Nodes und nicht über den Master.

\paragraph{Service}
Services bilden einen Kommunikationsweg für synchrone Kommunikation zwischen zwei Nodes. Ein Service wird durch ein Paar von zwei Messages definiert: Einer Request und einer Response. Auch Services werden beim Master angemeldet. Anders als Topics darf ein Service allerdings nur von einem Node inseriert werden. Andere Nodes können einen Service aufrufen und erhalten eine exklusive Antwort zurück.

\paragraph{Master}
Der ROS Master bietet einen Service zur Registrierung und Namensgebung für alle Nodes, Topics und Services. Er ist auf jedem ROS System vorhanden und wird von jedem Node gekannt. Dadurch ermöglicht er den anderen Nodes sich gegenseitig zu finden, um eine Peer-To-Peer Kommunikation aufzubauen.

\paragraph{Parameter Server}
Der Parameter Server läuft innerhalb des ROS Masters und dient Nodes,Parameter zu speichern und abzurufen. Diese sind in der Regel statische, nicht-binäre Daten wie Konfigurationsparameter.
Unter anderen wird die Beschreibung des Roboters in Form des Unified Robot Description Formats (URDF) oder des Semantic Robot Description Formats (SRDF) hier gespeichert. Diese Beschreibungen spezifizieren sowohl die kinematischen und dynamischen Eigenschaften, die visuelle Repräsentation und das Kollisionsmodell des Roboters~\cite{semantic_robot_description}.

\paragraph{Package}
Um in einem größeren System nicht alle Nodes manuell starten zu müssen, können sie in einem Package gebündelt und über eine Launch Datei gestartet werden. Die Launch Datei beschreibt die Startparameter der einzelnen Nodes und deren Abhängigkeit zu weiteren Nodes und Packages. Neben Nodes kann ein Package auch ROS-unabhängige Software, Konfigurationsdateien und Daten enthalten. Ziel von Packages ist die einfache Wiederverwendung von Softwaremodulen.

\subsection{MoveIt}
MoveIt ist das primäre Motion-Planning Framework in ROS und bietet eine relativ niedrige Einstiegshürde~\cite{coleman_reducing_2014}. Die Kernfunktionalitäten sind aus austauschbaren Komponenten aufgebaut. Als Standard Motion Planning Plugin wird die Open Motion Planning Library (OMPL), zur Kollisionserkennung die Fast Collision Library (FCL) und für die kinematischen Berechnungen die OROCOS Kinematics and Dynamics Library (KDL) verwendet \cite{chitta_moveitros_2012}.
Die Grundbausteine der MoveIt Architektur sind in Abbildung~\ref{fig:moveit_concepts} dargestellt und werden nachfolgend, auf Grundlage des Referenzbuchs von Anis Koubaa~\cite{koubaa_anis_2016} und der MoveIt Dokumentation~\cite{moveit_concepts_nodate} kurz erklärt. Verwendet wird in dieser Arbeit die Version \glqq MoveIt 1 - Melodic\grqq.

\begin{figure}
	\centering
	\includegraphics[height=\textheight, width=\textwidth, keepaspectratio]{images/moveit_pipeline.png} 
	\caption{High Level Architektur von MoveIt~\cite{moveit_concepts_nodate}}
	\label{fig:moveit_concepts}
\end{figure}

\paragraph{Move Group}
Die Move Group ist der zentrale Knoten der MoveIt Architektur. In ihm werden die anderen Komponenten zusammengeführt, um sie dem Nutzer gebündelt zur Verfügung stellen zu können. Zum Ausführen und Planen von Bewegungen, wird eine maschinenlesbare Beschreibung des Roboters benötigt. Diese kann von der Move Group als ROS Node vom ROS Parameter Server abgerufen werden.

\paragraph{Planning Scene}
Die Planning Scene repräsentiert den aktuellen Zustand des Roboters und dessen Umgebung und wird innerhalb der Move Group von einem Planning Scene Monitor gepflegt. Dieser überwacht jeweils ein Topic zum aktuellen Zustand des Roboters, zu Sensordaten und zu weiteren Geometrien beziehungsweise Objekten in der Welt. Durch die im Zustand des Roboters gespeicherten Gelenkwerte, kann die exakte Pose des Roboters festgestellt werden. Ein Objekt, das aufgenommen worden ist, wird fest mit dem virtuellen Modell des Roboters verbunden, sodass es in der weiteren Pfadplanung mit berücksichtigt werden kann. Die Umgebung kann sowohl mit Hilfe von externen Sensoren modelliert, als auch durch vom Nutzer erstellte Kollisionsobjekten beeinflusst werden. Das resultierende Modell der Umgebung kann anschließend auf zwei Arten repräsentiert werden~\cite{chitta_moveitros_2012}:
\begin{enumerate}
	\item Voxel: Die Welt wird in dreidimensionale Zellen aufgeteilt und der Zustand jeder Zelle kann entweder belegt, frei oder unbekannt sein\footnote{\url{http://wiki.ros.org/voxel\_grid}}
	
	\item Geometrische Primitive oder Netzmodelle: Eine Dreidimensionale Beschreibung von bekannten Objekten und Hindernissen
\end{enumerate}

\paragraph{Planning Pipeline}
Die Planning Pipeline verbindet Planning Request Adapters mit dem eigentlichen Motion Planner. Diese Adapter können genutzt werden, um Anfragen an den Planner vorzuverarbeiten und die resultierende Trajektorie nachzubearbeiten.

\paragraph{Controller}
Um die Trajektorie auf dem Roboter auszuführen, muss dieser ein \glqq FollowJointTrajectoryAction\grqq{} Interface implementiert haben, das von der Move Group angesteuert wird. In der Regel wird ein entsprechender Server vom Hersteller des Roboters bereitgestellt.

\begin{figure}
	\centering
	\includegraphics[height=0.95\textheight, width=\textwidth, keepaspectratio]{images/Ablaufdiagramm.pdf} 
	\caption{Ablaufdiagramme für die Aufgaben der zwei Cobots.}
	\label{fig:ablaufdiagramm}
\end{figure}

\subsection{Objektorientierter Entwurf}
Die beiden Roboter der Fallstudie werden als separate Entitäten behandelt und sollen unabhängig voneinander agieren. Die in der Aufgabenstellung festgelegten Handlungen wurden im Ablaufdiagramm ~\ref{fig:ablaufdiagramm} visualisiert und dahingehend erweitert, dass das Fehlschlagen einer Greifaktion zur Rückkehr in einen sicheren Zustand resultiert. Pro Aufgabe werden folgende Constraints benötigt:

\begin{enumerate}
	\item Startposition: Es sind keine Constraints benötigt, da keine Handlungen durchgeführt werden.
	\item Flasche greifen: Die Orientierung des Endeffektors muss horizontal zum Boden sein, damit die Flasche seitlich gegriffen werden kann. 
	\item Glas füllen: Während der Bewegung vom Aufnahmeort der Flasche zum Glas, bleibt die Orientierung beschränkt, um ein Ausschütten zu verhindern. Ebenso wird die Beschleunigung beschränkt, um ein Überschwappen zu vermeiden. Zum Befüllen des Glases wird das Orientierungs-Constraint aufgehoben, um eine Rotation des Endeffektors zu erlauben.
	\item Flasche abstellen: Die Orientierung muss wieder horizontal zum Boden sein, damit eventuelle Flüssigkeitsreste nicht verschüttet werden und ein korrektes Abstellen möglich ist. Eine Beschränkung der Beschleunigung ist nicht mehr notwendig und nachdem die Flasche abgestellt worden ist, muss auch die Orientierung nicht weiter beschränkt werden. Zusätzlich gilt es einen extra Sicherheitsabstand zur Tischplatte zu halten.
	\item Glas greifen: Nach erfolgreichem Greifen des befüllten Glases, werden Orientierung und Beschleunigung erneut beschränkt, um ein Verschütten zu verhindern.
	\item Safezone frei: Nachfolgende Handlungen können erst ausgeführt werden, wenn die Sicherheitszone zwischen den Robotern nicht vom anderen Roboter blockiert wird.
	\item Glas abstellen: Die Orientierung und Beschleunigung bleiben beschränkt, bis das Glas abgestellt worden ist.
\end{enumerate}
Die aufgelisteten Constraints gelten für beide Cobots.

Die Sicherheitszone zwischen den Robotern - einschließlich des Übergabeorts - wird von einer weiteren Entität kontrolliert. Will ein Roboter die Sicherheitszone betreten, muss er dieses Recht bei dem Controller anfordern. Der Controller sorgt dafür, dass immer nur ein Roboter dieses Recht erhält. Ist die Zone bereits belegt, gibt er eine negative Antwort zurück. Der abgelehnte Roboter kann anschließend beliebig oft neue Anfragen senden. Erst nachdem sich der erste Roboter wieder abmeldet, darf der zweite die Sicherheitszone betreten. Dieser Vorgang (einschließlich der Aktivierung der Roboter durch die Drucksensoren) ist im Sequenzdiagramm~\ref{fig:sequenzdiagramm} dargestellt.

\begin{figure}
	\centering
	\includegraphics[height=\textheight, width=\textwidth, keepaspectratio]{images/Sequenzdiagramm.pdf} 
	\caption{Rechtevergabe für die Sicherheitszone zwischen den zwei Cobots.}
	\label{fig:sequenzdiagramm}
\end{figure}


\begin{figure}
	\centering
	\includegraphics[height=0.95\textheight, width=\textwidth, keepaspectratio]{images/Klassendiagramm Cobot.pdf} .
	\caption{Entwurfsklassendiagramm des ersten Cobots}
	\label{fig:klassendiagramm}
\end{figure}

Aus der Entwurfssicht ergeben sich die im Entwurfsklassendiagramm in Abbildung~\ref{fig:klassendiagramm} dargestellten Entitäten. Das Klassendiagramm für den zweiten Cobot unterscheidet sich lediglich in der \textit{Cobot} Klasse, da dieser keine Handlungen mit der Flasche durchführt. Ein \textit{Cobot} kennt seine Startposition, die gleichzeitig als sicherer Zustand dient, alle Handlungen, die er prinzipiell ausführen kann und eine beliebige Anzahl an Constraints, die individuell angewandt oder entfernt werden können. Um die Anwendung an dieser Stelle für weitere Constraints erweiterbar zu halten, muss jeder Constraint die abstrakte Klasse \textit{Constraint} implementieren. Der \textit{SafezoneController} speichert den Zustand der Sicherheitszone und kann Zugang zu ihr entweder gewähren oder ablehnen. Ein Drucksensor stellt die Information über seinen Druckzustand zur Verfügung.

Da ein Cobot nicht weiß, wann und in welcher Reihenfolge er seine Handlungen ausführen soll, wird er von einem \textit{CobotController} gesteuert. Dieser implementiert eins der Ablaufdiagramme aus Abbildung~\ref{fig:ablaufdiagramm} und ist außerdem zuständig für die Kommunikation mit \textit{SafeZoneController} und \textit{PressureSensor} und dem Hinzufügen von Objekten in die PlanningScene mit Hilfe des \textit{ObjectCreators}. Entsprechend Anforderung A2 sollen diese Objekte konfigurierbar sein. Die drei Einheiten \textit{CobotController}, \textit{SafeZoneController} und \textit{PressureSensor} laufen unabhängig voneinander und können in einem ROS System als eigenständige Nodes implementiert werden. Dadurch kann die Kommunikation entsprechend der Abbildung~\ref{fig:node_communication} realisiert werden. 
\begin{figure}
	\centering
	\includegraphics[height=0.95\textheight, width=\textwidth, keepaspectratio]{images/NodeCommunication.pdf} 
	\caption{Kommunikation zwischen den Nodes}
	\label{fig:node_communication}
\end{figure}
Die Drucksensoren veröffentlichen ihren Zustand auf einem Topic, welches von dem \textit{CobotController} abonniert wird (gekennzeichnet durch den durchgezogenen Pfeil). Für die Anfrage an den \textit{SafezoneController} bietet eine Implementierung als ROS Service an (gekennzeichnet durch die gestrichelten Pfeile).


\section{Implementierung}
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 Implementierung in mehrere Packages aufgeteilt, die über CMake miteinander gelinkt werden. In den folgenden Abschnitten wird auf die zentralen Packages einzeln eingegangen.

\subsection{Cobot}\label{ch:cobot_impl}
Die Packages \textit{cobot\_1} und \textit{cobot\_2} enthalten jeweils die Implementierungen 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 vom Cobot 1 das Topic \glqq pressure\_1\grqq{} und vom Cobot 2 das Topic \glqq pressure\_2\grqq{} abonniert, um informiert zu werden, sobald das Glas auf dem 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 Implementierung einer solchen Aufgabe ist im Codebeispiel~\ref{lst:pick_bottle} dargestellt.

\lstinputlisting[firstline=29,lastline=41, label={lst:pick_bottle}, caption={Beispielimplementierung einer Cobot-Aufgabe}]{../cobot_ws/src/cobot_1/src/Cobot.cpp}

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 Implementierung, 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. Die Constraints werden im Konstruktor des \textit{Cobot}s als Klassenvariablen instanziiert und lassen sich so innerhalb der Handlungen anwenden und entfernen.

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.

Die Launch Datei des Packages startet neben dem \textit{CobotController} auch den \textit{SafezoneController}- und \textit{PressureSensor}-Node und eine Simulationsumgebung.

\subsection{Constraints}\label{ch:constraint_impl}
Das Package \textit{constraints} enthält die Definition der abstrakten \textit{Constraint}-Klasse und die vier konkreten Implementierungen \textit{AccelerationConstraint}, \textit{OrientationConstraint}, \textit{SafezoneConstraint} und \textit{VelocityConstraint}.

Der Orientierungs-Constraint (Codebeispiel~\ref{lst:orientation_constraint}) wird realisiert, indem eine\newline
 \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={Implementierung 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={Implementierung 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.

\lstinputlisting[firstline=17,lastline=32, label={lst:safezone_controller}, caption={Callback Funktion des SafezoneController}]{../cobot_ws/src/safe_zone_controller/src/SafeZoneController.cpp}

\subsection{ObjectCreator}\label{ch:objectCreator}
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 Teil des \textit{pressure\_sensor} Packages ist 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.