diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/app.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/app.c
index 4fa1948298b9449afc1885e75db7429ad9e08efa..c0e4d84f1d3f07135e2a04d6c9818a0b3167fd41 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/app.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/app.c
@@ -22,6 +22,7 @@
#include "app.h"
#include "sys/util/stubs.h"
#include "sys/sched/scheduler.h"
+#include "app/leddebug.h"
/************* Private typedefs ***********************************************************/
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/config.h b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/config.h
index cf6adc3a081a3f8d43a59880965b4b6d578b5ebf..9848bf631e61d8f59ada7672038850a25d0b7faa 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/config.h
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/config.h
@@ -29,7 +29,7 @@
//#define CAR_V1
#define CAR_V2
-#define USE_RX24F
+//#define USE_RX24F
#define WENC_V3 /* wheel encoder with tacho & direction output */
//#define Board
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangproc/src/stangproc.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangproc/src/stangproc.c
index c32aa18e487b9d097a6b2246634e072709bdbe39..a116a75d2e26625479cb390272e31833282a0adc 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangproc/src/stangproc.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangproc/src/stangproc.c
@@ -58,7 +58,6 @@ static float32_t transformSvrAngToStAng(float32_t servoAngAct);
/************* Public functions ***********************************************************/
int8_t stangproc_init(void)
{
-
stAngMax = 0;
stAngMin = 0;
stAngTgtFailSafe = 0;
@@ -93,7 +92,7 @@ void stangproc_step(void)
irq_enable();
/* call servo device driver to update to the new target value */
- #ifndef USE_RX24F
+ #ifndef USE_RX24F
servo_updateAngleSrv1();
#endif
}
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangsel/src/stangsel.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangsel/src/stangsel.c
index 5724a7d004a541dfe20c0e3d67c282b56b7f9004..795f93f5eb61b604fb7c76b134983c90e710fd08 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangsel/src/stangsel.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stangsel/src/stangsel.c
@@ -84,7 +84,6 @@ void stangsel_step(void)
break;
}
-
if(selStAng > SEL_STEER_ANGLE_MAX){
selStAng = SEL_STEER_ANGLE_MAX;
}
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stm/src/stm.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stm/src/stm.c
index 1eb571f204c168d37e073e32c6de64679b0efada..e7fae04b79d06b1f82c2e251b796a468e863b367 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stm/src/stm.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/stm/src/stm.c
@@ -82,15 +82,28 @@ void stm_step(void) {
switch (prevState)
{
case SYSTEM_STATE_INITIALIZING:
- irq_disable();
- /* syncOffset is initialized with 0, so if it is != 0 here, clocksync has been successful.
- * if clocksync successful: goto idle
- */
- if (*pSyncOffset_stm != 0)
- {
- state = SYSTEM_STATE_RUNNING_EXT;
- }
- irq_enable();
+
+ // ATTENTION
+ // ---------
+ // The following code segment was commented out due to the following reason:
+ // It only changes state when the syncOffset variable is set which will
+ // occur on an external interrupt from the Odroid.
+ // However, this interrupt will be only signaled once when the odroid boots.
+ // Since we don't use the syncOffset, we will just ignore this value and directly go to
+ // the running_ext state.
+
+ // irq_disable();
+ // /* syncOffset is initialized with 0, so if it is != 0 here, clocksync has been successful.
+ // * if clocksync successful: goto running_ext
+ // */
+ // if (*pSyncOffset_stm != 0)
+ // {
+ // state = SYSTEM_STATE_RUNNING_EXT;
+ // }
+
+ // irq_enable();
+
+ state = SYSTEM_STATE_RUNNING_EXT;
break;
case SYSTEM_STATE_IDLE:
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/vehspdctrl/src/vehspdctrl.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/vehspdctrl/src/vehspdctrl.c
index e6da5198fc1551f148b9ae7f36c5471eada1ddec..89e6529f8ad2f355539914f4179466ebf52ba119 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/vehspdctrl/src/vehspdctrl.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/app/vehspdctrl/src/vehspdctrl.c
@@ -152,7 +152,7 @@ float spdCtrl(float w)
float y = 0; // controlled value
irq_disable();
- y = -*pVehSpdOdom_vehspdctrl;
+ y = *pVehSpdOdom_vehspdctrl;
irq_enable();
y = pt1_calculate(&pt1, y);
diff --git a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/per/src/clocksync.c b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/per/src/clocksync.c
index 140e0df943154f4ef959b07f39a5b67ffd1f51ed..2b29170b3080e2b1bd8e3889812afc9dde241c1f 100755
--- a/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/per/src/clocksync.c
+++ b/attachments/Austausch Assystem - HU/02_lieferung/vehicle_ecu_sw/quellcode/per/src/clocksync.c
@@ -16,6 +16,7 @@
/************* Includes *******************************************************************/
#include "per/clocksync.h"
+#include "app/leddebug.h"
/************* Private typedefs ***********************************************************/
@@ -69,7 +70,6 @@ int8_t clocksync_initDevice(void)
void EXTI15_10_IRQHandler(void) {
/* Make sure that interrupt flag is set */
if (EXTI_GetITStatus(EXTI_Line13) != RESET) {
-
*pSyncOffset_clocksync = systime_getSysTime();
EXTI_ClearITPendingBit(EXTI_Line13);
diff --git a/doc/mainDocument/mainDocument.tex b/doc/mainDocument/mainDocument.tex
index d2a8db28d66626a91ef31d3580fdc91c4a799560..14379010520aa7cbe16353b48fcd006e4cd20244 100644
--- a/doc/mainDocument/mainDocument.tex
+++ b/doc/mainDocument/mainDocument.tex
@@ -123,8 +123,6 @@
\clearpage
\subfile{sections/usecases}
\clearpage
-\subfile{sections/setup}
-\clearpage
\subfile{sections/kommunikation}
\clearpage
\subfile{sections/architektur}
diff --git a/doc/mainDocument/sections/auswertung.tex b/doc/mainDocument/sections/auswertung.tex
index 5cd80824088b803271e94636e43efe8a8be6f1af..4d234803abe75542cdf8ee59c84179e0d96f31e4 100644
--- a/doc/mainDocument/sections/auswertung.tex
+++ b/doc/mainDocument/sections/auswertung.tex
@@ -57,20 +57,20 @@
\item Usecase
\item Kommunikationsprotokoll Design
\item Systemarchitektur Design
- \item Testplan Design
+ \item Testplan Design
\item Ultraschallsensor Implementation
\end{itemize}
- \subsubsection{Phuc Truong}
+ \subsubsection{Phuc Tran Truong}
\begin{itemize}
\item Dokumentation
\item Usecase
- \item Odroid Installation und Konfiguration
+ \item ODROID Installation und Konfiguration
\end{itemize}
\subsubsection{Noel Dan Le}
\begin{itemize}
\item Dokumentation
\item MavLink
- \item Odroid Netzwerkkonfiguration
+ \item ODROID Netzwerkkonfiguration
\end{itemize}
\end{document}
diff --git a/doc/mainDocument/sections/projekt.tex b/doc/mainDocument/sections/projekt.tex
index ae4070f7fec7104ccd76794274ecdedaeb8a3208..ecf85ce384d0fbec8f960ac2ff9a32d7204f2de0 100644
--- a/doc/mainDocument/sections/projekt.tex
+++ b/doc/mainDocument/sections/projekt.tex
@@ -41,6 +41,182 @@
\end{figure}
\subsection{Setup}
- \subsubsection{Netzwerk}
+In diesem Abschnitt sollen die Schritte beschrieben werden, um das ODROID vollständig einzurichten. Auf das Speichermodul, die eMMC, muss das Betriebssystem Ubuntu 16.04 LTS gespielt werden. Das WLAN (eduroam) muss eingerichtet werden, um grundlegende Tools wie vim und git und die ROS Software Bibliotheken/Werkzeuge zu installieren und die Repositories der Gruppen zu klonen.
+
+\subsubsection{ODROID und Ubuntu}
+Um das ODROID nutzen zu können, müssen wir das Betriebssystem Ubuntu 16.04 LTS installieren. Dazu benötigen wir einen Computer, die eMMC und einen microSD-Adapter.
+\begin{figure}[h]
+\centering
+\begin{minipage}{.4\textwidth}
+ \centering
+ \includegraphics[width=2cm]{emmc}
+ \captionof{figure}{ODROID-XU3/XU4 eMMC 5.0 Modul, 32 GB}
+ \label{fig:test1}
+\end{minipage}%
+\hfill
+\begin{minipage}{.4\textwidth}
+ \centering
+ \includegraphics[width=2cm]{emmcAdapter}
+ \captionof{figure}{eMMC/microSD Kartenleser-Adapter}
+ \label{fig:test2}
+\end{minipage}
+\end{figure}\\
+Eine Anleitung zur Installation von Ubuntu auf dem ODROID XU4 gibt es hier:
+\url{https://scroom.de/ubuntu-16-04-lts-auf-dem-odroid-xu4-installieren/}
+
+Als erstes muss man die eMMC mittels microSD-Adapter an einen Computer anschließen. Diese muss man formatieren.
+
+Dann muss man die für das ODROID passende Ubuntu Distribution von Hardkernel herunterladen:
+\url{https://wiki.odroid.com/odroid-xu4/os_images/linux/ubuntu/ubuntu}
+
+Dieses Image mittels unxz entpacken:
+
+\lstinline{unxz ubuntu-16.04.3-4.14-minimal-odroid-xu4-20171213.img.xz}
+
+Um nun Ubuntu auf das eMMC zu flashen, benutzt man am besten Etcher. Alternativ kann man diesen Vorgang auch mit Konsolenbefehlen durchführen. Diese findet man in der oben angegebenen Anleitung.
+Für Linux Systeme gibt es Etcher als .AppImage Datei: \url{https://etcher.io}
+
+Um diese auszuführen:
+\begin{enumerate}
+\item Das AppImage ausführbar machen:
+
+\lstinline{chmod a+x etcher-1.3.1-x86_64.AppImage}
+\item Das Programm ausführen:
+
+\lstinline{./etcher-1.3.1-x86_64.AppImage}
+\end{enumerate}
+Dann wählt man das Ubuntu Image und die eMMC aus. Dann führt Etcher den Flashvorgang inklusive Validierung durch. Dann ist es möglich, vom Computer aus auf das Dateisystem auf der eMMC zuzugreifen. Skripte oder andere Dateien, die man zur Einrichtung des ODROIDs benötigt oder die die Einrichtung erleichtern, kann man also schon an dieser Stelle auf die eMMC übertragen.
+Anschließend kann man die eMMC vom Computer trennen und an das ODROID anschließen.
+Dann sollte es booten.
+
+
+Standardmäßig kann man sich mit dem Nutzernamen \textit{root} und dem Passwort \textit{odroid} anmelden.
+Um das Passwort zu ändern, führt man den folgenden Befehl aus: \lstinline{sudo passwd}. Für das Semesterprojekt lautet das Passwort bei jedem fertig eingerichteten ODROID \glqq autonom1718\grqq.
+
+\subsubsection{WLAN (eduroam)}
+Um eine WLAN-Verbindung herzustellen, benötigt man einen WLAN-Stick oder ein WLAN-Modul. Wir gehen an dieser Stelle davon aus, dass diese nach dem Plug-and-Play-Prinzip funktionieren und hier keine Einrichtung nötig ist.
+Man kann mittels \lstinline{ifconfig} und \lstinline{iwconfig} überprüfen, ob die WLAN-Schnittstelle vorhanden ist und wie sie heißt. Wir gehen von nun an davon aus, dass die Schnittstelle \textit{wlan0} heißt. Andernfalls muss man den Namen der Schnittstelle im Folgenden immer anpassen.
+
+Der herkömmliche Weg, wie man sich mit einem WLAN-Netzwerk verbindet:
+\begin{enumerate}
+\item Mittels \lstinline{ifconfig wlan0 up} die WLAN-Karte anschalten.
+\item Mit \lstinline{iwconfig wlan0 essid name key password} mit dem Netzwerk verbinden. Ersetze \textit{name} dem Namen des Netzwerks, \textit{password} mit dem Sicherheitsschlüssel des Netzwerks.
+\item Mit \lstinline{dhclient wlan0} eine IP Adresse erhalten und mit dem WLAN- Netzwerk verbinden.
+\end{enumerate}
+Die WLAN-Verbindung kann man mittels \lstinline{iwconfig} und \lstinline{ping www.hu-berlin.de} überprüfen.
+
+
+Um sich mit dem eduroam zu verbinden, sind andere Schritte nötigt. Die Einrichtung wird in der folgenden Anleitung ausführlich beschrieben:
+\url{https://www.cms.hu-berlin.de/de/dl/netze/wlan/config/eduroam/linux/eduroam-linux-wpa_supp.pdf}
+
+Für die Einrichtung sind zwei Dateien nötig:
+\begin{enumerate}
+\item \href{http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/deutsche-telekom-root-ca-2.crt}{\texttt{deutsche-telekom-root-ca-2.crt}}
+\item \href{http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/linux/wpa_supplicant.conf}{\texttt{wpa\_supplicant.conf}}
+\end{enumerate}
+Am besten lädt man diese Dateien mit einem anderen Computer herunter und kopiert diese (z.B. nach dem Flashvorgang) auf das ODROID. Ansonsten kann man die Dateien auf einen USB-Stick kopieren und diese auf das ODROID übertragen. Um auf den USB-Stick zuzugreifen, führt man folgende Schritte durch:
+\begin{enumerate}
+\item Mit \lstinline{lsblk} herausfinden, wie der USB-Stick heißt. Die Partition sollte in etwa so aussehen wie \lstinline{/dev/sdb1}.
+\item Mit \lstinline{sudo mkdir /media/usb} einen Mount Point (Einhängepunkt) erstellen.
+\item Mit \lstinline{sudo mount /dev/sdb1 /media/usb} mounten. Ãœber den Ordner \lstinline{/media/usb} kann man nun auf die Dateien im USB-Stick zugreifen.
+\item Mit \lstinline{sudo umount /media/usb} unmounten, wenn man nicht mehr länger auf den USB-Stick zugreifen will.
+\end{enumerate}
+Alternativ lädt man die Dateien mittels \lstinline{curl} oder \lstinline{wget} vorher mit einer WLAN-Verbindung zu Hause oder via Hotspot herunter. Z.B.:
+\begin{itemize}
+\item \lstinline{wget http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/deutsche-telekom-root-ca-2.crt}
+\item \lstinline{curl -O http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/linux/wpa_supplicant.conf}
+\end{itemize}
+In jedem Fall muss man die zwei Dateien in den \lstinline{/etc/wpa_supplicant} Ordner bewegen (oder mit \lstinline{cp} kopieren). Dabei muss \lstinline{/path/to} durch den entsprechenden Pfad zu den Dateien ersetzt werden.
+\begin{itemize}
+\item \lstinline{sudo mv /path/to/deutsche-telekom-root-ca-2.crt /etc/wpa_supplicant/}
+\item \lstinline{sudo mv /path/to/wpa_supplicant.conf /etc/wpa_supplicant}
+\end{itemize}
+Nun muss man \lstinline{wpa_supplicant.conf} bearbeiten. Am einfachsten ist es, diese Datei schon vorher auf einem Computer zu bearbeiten. Ansonsten muss man \lstinline{vi /etc/wpa_supplicant/wpa_supplicant.conf} ausführen. Außer \lstinline{vi} ist standardmäßig kein anderer Editor auf dem ODROID installiert. Ansonsten kann man sich schon vorher via Hotspot oder zu Hause mittels \lstinline{sudo apt install vim} den Editor \lstinline{vim} installieren. Hier ist eine Anleitung, wie man \lstinline{vi} verwendet: \url{https://www.suse.com/de-de/documentation/sles10/book_sle_reference/data/sec.suse.vim.html}.
+
+In der \lstinline{wpa_supplicant.conf} muss man \textit{YourNameHere} durch die HU E-Mail Adresse ersetzen, also etwas wie \textit{foobar@informatik.hu-berlin.de}. \textit{YourPasswordHere} kann man durch das entsprechende Passwort ersetzen. Allerdings steht das Passwort in der Datei als Klartext und ist für jeden sichtbar, der auf die Datei als root User zugreifen kann. Alternativ kann man die Passwortzeile löschen oder auskommentieren. Dann muss man das Passwort jedes Mal eingeben, wenn man sich mit eduroam verbinden will.
+
+Um die Verbindung zu testen, führt man nun folgende Befehle aus:
+\begin{enumerate}
+\item \lstinline{sudo service network-manager stop}, um eventuelle automatische Konfigurationsversuche zu vermeiden
+\item \lstinline{sudo wpa_supplicant -i wlan0 -D wext -c /etc/wpa_supplicant/wpa_supplicant.conf -d}
+\end{enumerate}
+Falls die Passwortzeile gelöscht wurde, muss man nun in einer neuen Konsole Ctrl+Alt+F2
+\lstinline{sudo wpa_cli} ausführen. Nach kurzer Zeit sollte eine Zeile angezeigt werden, die sagt, dass für eduroam ein Passwort nötig ist. Dann gibt man \lstinline{password 0 YourPassword} ein, wobei \textit{YourPassword} durch das entsprechende HU Passwort ersetzt werden muss. Falls die Eingabe akzeptiert wird, dann sollte eine Zeile angezeigt werden, die sagt, dass die Verbindung erfolgreich war. Aus diesem interaktiven Modus kommt man mittels \lstinline{q} heraus.
+Nun kann man die WLAN-Verbindung wie zuvor mittels \lstinline{iwconfig} oder \lstinline{ping www.hu-berlin.de} überprüfen. Falls dies noch nicht funktioniert, führt man noch folgende Befehle aus, um eine neue IP Adresse zu erhalten und sich mit dem WLAN-Netzwerk zu verbinden.:
+\begin{enumerate}
+\item \lstinline{dhclient wlan0 -r}
+\item \lstinline{dhclient wlan0}
+\end{enumerate}
+
+Um die Änderungen permanent zu machen, bearbeiten wir \lstinline{/etc/network/interfaces}. Die Einstellungen für das WLAN-Interface sollten wie folgt aussehen:
+\begin{lstlisting}
+auto wlan0
+iface wlan0 inet dhcp
+wpa-conf /etc/wpa_supplicant/wpa_supplicant.conf
+\end{lstlisting}
+Beim nächsten Hochfahren des ODROIDs muss man, falls die Passwortzeile gelöscht wurde, den Befehl \lstinline{sudo wpa_cli} ausführen und wie zuvor beschrieben das Passwort eingeben, um sich mit dem eduroam zu verbinden.
+
+Nun ist es möglich, das ODROID per \lstinline{ssh} zu steuern. Die IP-Adresse des ODROIDs erfährt man mit \lstinline{ifconfig wlan0}. Anschließend kann man sich mit \lstinline{ssh root@10.42.43.1} verbinden. Die IP-Adresse muss hier entsprechend angepasst werden. Mit \lstinline{scp} ist es außerdem möglich, Daten zu übertragen.
+Bsp.: \lstinline{scp foobar.txt root@10.42.43.1:/some/remote/directory}
+
+\subsubsection{Essentials}
+Nun da das ODROID eine Internetverbindung hat, sollte man es updaten: \lstinline{sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade}
+
+Anschließend sollte \lstinline{git} installiert werden, damit die Repositories der Gruppen mit der Software geklont werden können: \lstinline{sudo apt install git-all}
+
+Der Befehl, um ein Repository zu klonen, sieht dann z.B. so aus:\\
+\lstinline{git clone https://gitlab.informatik.hu-berlin.de/adapt/ws17-HF/Gruppe-A.git}\\
+Beim Klonen, Pullen, Pushen usw. wird jedes Mal der Nutzername und das Passwort abgefragt.
+
+\subsubsection{ROS}
+Für das Semesterprojekt wird ROS (Robot Operating System) verwendet. Konkret wird die ROS Version Lunar Loggerhead verwendet. Eine ausführliche Anleitung zur Installation gibt es hier: \url{http://wiki.ros.org/lunar/Installation/Ubuntu}
+
+Es empfiehlt sich, vorher auf einem Computer (mit GUI und Browser) ein Skript mit den folgenden Befehlen vorzubereiten und dieses Skript auf auf das ODROID zu kopieren, damit man dieses nur noch ausführen muss. Ansonsten muss man die Befehle manuell eintippen.
+\begin{enumerate}
+\item Das ODROID so einstellen, dass er Software von packags.ros.org akzeptiert:\\
+\lstinline{sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'}
+\item Die Schlüssel einstellen:\\
+\lstinline{sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key}\\ \lstinline{421C365BD9FF1F717815A3895523BAEEB01FA116}
+\item Den Debian Package Index aktualisieren: \lstinline{sudo apt-get update}
+\item ROS-Base installieren: \lstinline{sudo apt-get install ros-lunar-ros-base}
+\item \lstinline{rosdep} initialisieren:\\
+\lstinline{sudo rosdep init}\\
+\lstinline{rosdep update}
+\item Die Umgebung einrichten, damit ROS Umgebungsvariablen automatisch zur Bash Session hinzugefügt werden:\\
+\lstinline{echo "source /opt/ros/lunar/setup.bash" >> ~/.bashrc}\\
+\lstinline{source ~/.bashrc}
+\item Werkzeuge und andere Abhängigkeiten zum Bauen von ROS Packages installieren:\\
+\lstinline{sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential}
+\end{enumerate}
+Alle weiteren Schritte mit ROS werden in den ROS Tutorials beschrieben:\\
+\url{http://wiki.ros.org/ROS/Tutorials}
+
+\subsubsection{AdHoc Netzwerk}
+Man kann die ODROIDs über ein gemeinsames WLAN-Netzwerk kommunizieren lassen. In einem Szenario, wo echte Autos in Kolonne fahren sollen, ist so ein WLAN-Netzwerk jedoch meist nicht vorhanden.
+Für die drahtlose Kommunikation zwischen den ODROIDs verwenden wir daher ein AdHoc Netzwerk.
+Dafür empfiehlt es sich, ein Skript zu erstellen.
+\begin{lstlisting}
+IFACE="wlan0"
+
+sudo service network-manager stop
+
+sudo ifconfig $IFACE down
+sudo iw $IFACE set type ibss
+sudo ifconfig $IFACE up
+
+sudo iw $IFACE ibss join SPHAF 2432
+sudo ifconfig $IFACE 10.42.43.1
+\end{lstlisting}
+Dies nennen wir \lstinline{connectAdHoc.sh}. Mittels \lstinline{chmod u+x connectAdHoc.sh} macht man das Skript ausführbar und kann es dann mit \lstinline{./connectAdHoc.sh} ausführen.
+Mit \lstinline{iwconfig} kann man überprüfen, ob alles funktioniert hat.
+
+Mit dem Skript wird ein AdHoc Netzwerk mit dem Namen \textit{SPHAF} (SemesterProjekt HochAutonomes Fahren) auf dem Kanal 5 erstellt und das ODROID erhält die IP-Adresse 10.42.43.1. Für jeden Teilnehmer führt man das Skript durch. Dabei sollte jeder Teilnehmer am AdHoc Netzwerk eine eigene IP-Adresse erhalten. Die letzte Zeile sollte also für jeden Teilnehmer angepasst werden.
+
+Nun kann man mit \lstinline{crontab} (\url{https://wiki.ubuntuusers.de/Cron/}) einstellen, dass das Skript jedes Mal direkt beim Neustart (vor dem Login-Vorgang) ausgeführt wird. Dies ist insbesondere dann hilfreich, wenn das ODROID auf dem Auto montiert ist und keine Tastatur angeschlossen ist.
+Dazu muss man nun \lstinline{crontab -e} ausführen und die folgende Zeile einfügen: \lstinline{@reboot /root/connectAdHoc.sh}. Eventuell ist noch folgender Schritt nötig, um crond automatisch beim Hochfahren zu starten: \lstinline{update-rc.d cron defaults}.
+
+Falls das AdHoc Netzwerk steht, kann man sich mittels \lstinline{ssh root@10.42.43.1} mit dem ODROID verbinden.
+Bei uns kam es vor, dass das AdHoc Netzwerk gelegentlich abgestürzt ist.
+In \lstinline{crontab -e} kann man dann noch folgende Zeile einfügen, um das Adhoc Netzwerk alle 2 Minuten neuzustarten bzw., das entsprchende Skript auszuführen: \lstinline{*/2 * * * * /root/connectAdHoc.sh}.
\end{document}
\ No newline at end of file
diff --git a/doc/mainDocument/sections/setup.tex b/doc/mainDocument/sections/setup.tex
deleted file mode 100644
index 1c0cf76dddda6b3760d7db5c7134d66040ed4a82..0000000000000000000000000000000000000000
--- a/doc/mainDocument/sections/setup.tex
+++ /dev/null
@@ -1,181 +0,0 @@
-\documentclass[../mainDocument.tex]{subfiles}
-\begin{document}
- \section{Setup}
-In diesem Abschnitt sollen die Schritte beschrieben werden, um das ODROID vollständig einzurichten. Auf das Speichermodul, die eMMC, muss das Betriebssystem Ubuntu 16.04 LTS gespielt werden. Das WLAN (eduroam) muss eingerichtet werden, um grundlegende Tools wie vim und git und die ROS Software Bibliotheken/Werkzeuge zu installieren und die Repositories der Gruppen zu klonen.
-
-\subsection{ODROID und Ubuntu}
-Um das ODROID nutzen zu können, müssen wir das Betriebssystem Ubuntu 16.04 LTS installieren. Dazu benötigen wir einen Computer, die eMMC und einen microSD-Adapter.
-\begin{figure}
-\centering
-\begin{minipage}{.4\textwidth}
- \centering
- \includegraphics[width=2cm]{emmc}
- \captionof{figure}{ODROID-XU3/XU4 eMMC 5.0 Modul, 32 GB}
- \label{fig:test1}
-\end{minipage}%
-\hfill
-\begin{minipage}{.4\textwidth}
- \centering
- \includegraphics[width=2cm]{emmcAdapter}
- \captionof{figure}{eMMC/microSD Kartenleser-Adapter}
- \label{fig:test2}
-\end{minipage}
-\end{figure}\\
-Eine Anleitung zur Installation von Ubuntu auf dem ODROID XU4 gibt es hier:
-\url{https://scroom.de/ubuntu-16-04-lts-auf-dem-odroid-xu4-installieren/}
-
-Als erstes muss man die eMMC mittels microSD-Adapter an einen Computer anschließen. Diese muss man formatieren.
-
-Dann muss man die für das ODROID passende Ubuntu Distribution von Hardkernel herunterladen:
-\url{https://wiki.odroid.com/odroid-xu4/os_images/linux/ubuntu/ubuntu}
-
-Dieses Image mittels unxz entpacken:
-
-\lstinline{unxz ubuntu-16.04.3-4.14-minimal-odroid-xu4-20171213.img.xz}
-
-Um nun Ubuntu auf das eMMC zu flashen, benutzt man am besten Etcher. Alternativ kann man diesen Vorgang auch mit Konsolenbefehlen durchführen. Diese findet man in der oben angegebenen Anleitung.
-Für Linux Systeme gibt es Etcher als .AppImage Datei: \url{https://etcher.io}
-
-Um diese auszuführen:
-\begin{enumerate}
-\item Das AppImage ausführbar machen:
-
-\lstinline{chmod a+x etcher-1.3.1-x86_64.AppImage}
-\item Das Programm ausführen:
-
-\lstinline{./etcher-1.3.1-x86_64.AppImage}
-\end{enumerate}
-Dann wählt man das Ubuntu Image und die eMMC aus. Dann führt Etcher den Flashvorgang inklusive Validierung durch. Dann ist es möglich, vom Computer aus auf das Dateisystem auf der eMMC zuzugreifen. Skripte oder andere Dateien, die man zur Einrichtung des ODROIDs benötigt oder die die Einrichtung erleichtern, kann man also schon an dieser Stelle auf die eMMC übertragen.
-Anschließend kann man die eMMC vom Computer trennen und an das ODROID anschließen.
-Dann sollte es booten.
-
-
-Standardmäßig kann man sich mit dem Nutzernamen \textit{root} und dem Passwort \textit{odroid} anmelden.
-Um das Passwort zu ändern, führt man den folgenden Befehl aus: \lstinline{sudo passwd}. Für das Semesterprojekt lautet das Passwort bei jedem fertig eingerichteten ODROID \glqq autonom1718\grqq.
-
-\subsection{WLAN (eduroam)}
-Um eine WLAN-Verbindung herzustellen, benötigt man einen WLAN-Stick oder ein WLAN-Modul. Wir gehen an dieser Stelle davon aus, dass diese nach dem Plug-and-Play-Prinzip funktionieren und hier keine Einrichtung nötig ist.
-Man kann mittels \lstinline{ifconfig} und \lstinline{iwconfig} überprüfen, ob die WLAN-Schnittstelle vorhanden ist und wie sie heißt. Wir gehen von nun an davon aus, dass die Schnittstelle \textit{wlan0} heißt. Andernfalls muss man den Namen der Schnittstelle im Folgenden immer anpassen.
-
-Der herkömmliche Weg, wie man sich mit einem WLAN-Netzwerk verbindet:
-\begin{enumerate}
-\item Mittels \lstinline{ifconfig wlan0 up} die WLAN-Karte anschalten.
-\item Mit \lstinline{iwconfig wlan0 essid name key password} mit dem Netzwerk verbinden. Ersetze \textit{name} dem Namen des Netzwerks, \textit{password} mit dem Sicherheitsschlüssel des Netzwerks.
-\item Mit \lstinline{dhclient wlan0} eine IP Adresse erhalten und mit dem WLAN- Netzwerk verbinden.
-\end{enumerate}
-Die WLAN-Verbindung kann man mittels \lstinline{iwconfig} und \lstinline{ping www.hu-berlin.de} überprüfen.
-
-
-Um sich mit dem eduroam zu verbinden, sind andere Schritte nötigt. Die Einrichtung wird in der folgenden Anleitung ausführlich beschrieben:
-\url{https://www.cms.hu-berlin.de/de/dl/netze/wlan/config/eduroam/linux/eduroam-linux-wpa_supp.pdf}
-
-Für die Einrichtung sind zwei Dateien nötig:
-\begin{enumerate}
-\item \href{http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/deutsche-telekom-root-ca-2.crt}{\texttt{deutsche-telekom-root-ca-2.crt}}
-\item \href{http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/linux/wpa_supplicant.conf}{\texttt{wpa\_supplicant.conf}}
-\end{enumerate}
-Am besten lädt man diese Dateien mit einem anderen Computer herunter und kopiert diese (z.B. nach dem Flashvorgang) auf das ODROID. Ansonsten kann man die Dateien auf einen USB-Stick kopieren und diese auf das ODROID übertragen. Um auf den USB-Stick zuzugreifen, führt man folgende Schritte durch:
-\begin{enumerate}
-\item Mit \lstinline{lsblk} herausfinden, wie der USB-Stick heißt. Die Partition sollte in etwa so aussehen wie \lstinline{/dev/sdb1}.
-\item Mit \lstinline{sudo mkdir /media/usb} einen Mount Point (Einhängepunkt) erstellen.
-\item Mit \lstinline{sudo mount /dev/sdb1 /media/usb} mounten. Ãœber den Ordner \lstinline{/media/usb} kann man nun auf die Dateien im USB-Stick zugreifen.
-\item Mit \lstinline{sudo umount /media/usb} unmounten, wenn man nicht mehr länger auf den USB-Stick zugreifen will.
-\end{enumerate}
-Alternativ lädt man die Dateien mittels \lstinline{curl} oder \lstinline{wget} vorher mit einer WLAN-Verbindung zu Hause oder via Hotspot herunter. Z.B.:
-\begin{itemize}
-\item \lstinline{wget http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/deutsche-telekom-root-ca-2.crt}
-\item \lstinline{curl -O http://www.cms.hu-berlin.de/dl/netze/wlan/config/eduroam/linux/wpa_supplicant.conf}
-\end{itemize}
-In jedem Fall muss man die zwei Dateien in den \lstinline{/etc/wpa_supplicant} Ordner bewegen (oder mit \lstinline{cp} kopieren). Dabei muss \lstinline{/path/to} durch den entsprechenden Pfad zu den Dateien ersetzt werden.
-\begin{itemize}
-\item \lstinline{sudo mv /path/to/deutsche-telekom-root-ca-2.crt /etc/wpa_supplicant/}
-\item \lstinline{sudo mv /path/to/wpa_supplicant.conf /etc/wpa_supplicant}
-\end{itemize}
-Nun muss man \lstinline{wpa_supplicant.conf} bearbeiten. Am einfachsten ist es, diese Datei schon vorher auf einem Computer zu bearbeiten. Ansonsten muss man \lstinline{vi /etc/wpa_supplicant/wpa_supplicant.conf} ausführen. Außer \lstinline{vi} ist standardmäßig kein anderer Editor auf dem ODROID installiert. Ansonsten kann man sich schon vorher via Hotspot oder zu Hause mittels \lstinline{sudo apt install vim} den Editor \lstinline{vim} installieren. Hier ist eine Anleitung, wie man \lstinline{vi} verwendet: \url{https://www.suse.com/de-de/documentation/sles10/book_sle_reference/data/sec.suse.vim.html}.
-
-In der \lstinline{wpa_supplicant.conf} muss man \textit{YourNameHere} durch die HU E-Mail Adresse ersetzen, also etwas wie \textit{foobar@informatik.hu-berlin.de}. \textit{YourPasswordHere} kann man durch das entsprechende Passwort ersetzen. Allerdings steht das Passwort in der Datei als Klartext und ist für jeden sichtbar, der auf die Datei als root User zugreifen kann. Alternativ kann man die Passwortzeile löschen oder auskommentieren. Dann muss man das Passwort jedes Mal eingeben, wenn man sich mit eduroam verbinden will.
-
-Um die Verbindung zu testen, führt man nun folgende Befehle aus:
-\begin{enumerate}
-\item \lstinline{sudo service network-manager stop}, um eventuelle automatische Konfigurationsversuche zu vermeiden
-\item \lstinline{sudo wpa_supplicant -i wlan0 -D wext -c /etc/wpa_supplicant/wpa_supplicant.conf -d}
-\end{enumerate}
-Falls die Passwortzeile gelöscht wurde, muss man nun in einer neuen Konsole Ctrl+Alt+F2
-\lstinline{sudo wpa_cli} ausführen. Nach kurzer Zeit sollte eine Zeile angezeigt werden, die sagt, dass für eduroam ein Passwort nötig ist. Dann gibt man \lstinline{password 0 YourPassword} ein, wobei \textit{YourPassword} durch das entsprechende HU Passwort ersetzt werden muss. Falls die Eingabe akzeptiert wird, dann sollte eine Zeile angezeigt werden, die sagt, dass die Verbindung erfolgreich war. Aus diesem interaktiven Modus kommt man mittels \lstinline{q} heraus.
-Nun kann man die WLAN-Verbindung wie zuvor mittels \lstinline{iwconfig} oder \lstinline{ping www.hu-berlin.de} überprüfen. Falls dies noch nicht funktioniert, führt man noch folgende Befehle aus, um eine neue IP Adresse zu erhalten und sich mit dem WLAN-Netzwerk zu verbinden.:
-\begin{enumerate}
-\item \lstinline{dhclient wlan0 -r}
-\item \lstinline{dhclient wlan0}
-\end{enumerate}
-
-Um die Änderungen permanent zu machen, bearbeiten wir \lstinline{/etc/network/interfaces}. Die Einstellungen für das WLAN-Interface sollten wie folgt aussehen:
-\begin{lstlisting}
-auto wlan0
-iface wlan0 inet dhcp
-wpa-conf /etc/wpa_supplicant/wpa_supplicant.conf
-\end{lstlisting}
-Beim nächsten Hochfahren des ODROIDs muss man, falls die Passwortzeile gelöscht wurde, den Befehl \lstinline{sudo wpa_cli} ausführen und wie zuvor beschrieben das Passwort eingeben, um sich mit dem eduroam zu verbinden.
-
-Nun ist es möglich, das ODROID per \lstinline{ssh} zu steuern. Die IP-Adresse des ODROIDs erfährt man mit \lstinline{ifconfig wlan0}. Anschließend kann man sich mit \lstinline{ssh root@10.42.43.1} verbinden. Die IP-Adresse muss hier entsprechend angepasst werden. Mit \lstinline{scp} ist es außerdem möglich, Daten zu übertragen.
-Bsp.: \lstinline{scp foobar.txt root@10.42.43.1:/some/remote/directory}
-
-\subsection{Essentials}
-Nun da das ODROID eine Internetverbindung hat, sollte man es updaten: \lstinline{sudo apt-get update && sudo apt-get upgrade && sudo apt-get dist-upgrade}
-
-Anschließend sollte \lstinline{git} installiert werden, damit die Repositories der Gruppen mit der Software geklont werden können: \lstinline{sudo apt install git-all}
-
-Der Befehl, um ein Repository zu klonen, sieht dann z.B. so aus:\\
-\lstinline{git clone https://gitlab.informatik.hu-berlin.de/adapt/ws17-HF/Gruppe-A.git}\\
-Beim Klonen, Pullen, Pushen usw. wird jedes Mal der Nutzername und das Passwort abgefragt.
-
-\subsection{ROS}
-Für das Semesterprojekt wird ROS (Robot Operating System) verwendet. Konkret wird die ROS Version Lunar Loggerhead verwendet. Eine ausführliche Anleitung zur Installation gibt es hier: \url{http://wiki.ros.org/lunar/Installation/Ubuntu}
-
-Es empfiehlt sich, vorher auf einem Computer (mit GUI und Browser) ein Skript mit den folgenden Befehlen vorzubereiten und dieses Skript auf auf das ODROID zu kopieren, damit man dieses nur noch ausführen muss. Ansonsten muss man die Befehle manuell eintippen.
-\begin{enumerate}
-\item Das ODROID so einstellen, dass er Software von packags.ros.org akzeptiert:\\
-\lstinline{sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'}
-\item Die Schlüssel einstellen:\\
-\lstinline{sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key}\\ \lstinline{421C365BD9FF1F717815A3895523BAEEB01FA116}
-\item Den Debian Package Index aktualisieren: \lstinline{sudo apt-get update}
-\item ROS-Base installieren: \lstinline{sudo apt-get install ros-lunar-ros-base}
-\item \lstinline{rosdep} initialisieren:\\
-\lstinline{sudo rosdep init}\\
-\lstinline{rosdep update}
-\item Die Umgebung einrichten, damit ROS Umgebungsvariablen automatisch zur Bash Session hinzugefügt werden:\\
-\lstinline{echo "source /opt/ros/lunar/setup.bash" >> ~/.bashrc}\\
-\lstinline{source ~/.bashrc}
-\item Werkzeuge und andere Abhängigkeiten zum Bauen von ROS Packages installieren:\\
-\lstinline{sudo apt-get install python-rosinstall python-rosinstall-generator python-wstool build-essential}
-\end{enumerate}
-Alle weiteren Schritte mit ROS werden in den ROS Tutorials beschrieben:\\
-\url{http://wiki.ros.org/ROS/Tutorials}
-
-\subsection{AdHoc Netzwerk}
-Man kann die ODROIDs über ein gemeinsames WLAN-Netzwerk kommunizieren lassen. In einem Szenario, wo echte Autos in Kolonne fahren sollen, ist so ein WLAN-Netzwerk jedoch meist nicht vorhanden.
-Für die drahtlose Kommunikation zwischen den ODROIDs verwenden wir daher ein AdHoc Netzwerk.
-Dafür empfiehlt es sich, ein Skript zu erstellen.
-\begin{lstlisting}
-IFACE="wlan0"
-
-sudo service network-manager stop
-
-sudo ifconfig $IFACE down
-sudo iw $IFACE set type ibss
-sudo ifconfig $IFACE up
-
-sudo iw $IFACE ibss join SPHAF 2432
-sudo ifconfig $IFACE 10.42.43.1
-\end{lstlisting}
-Dies nennen wir \lstinline{connectAdHoc.sh}. Mittels \lstinline{chmod u+x connectAdHoc.sh} macht man das Skript ausführbar und kann es dann mit \lstinline{./connectAdHoc.sh} ausführen.
-Mit \lstinline{iwconfig} kann man überprüfen, ob alles funktioniert hat.
-
-Mit dem Skript wird ein AdHoc Netzwerk mit dem Namen \textit{SPHAF} (SemesterProjekt HochAutonomes Fahren) auf dem Kanal 5 erstellt und das ODROID erhält die IP-Adresse 10.42.43.1. Für jeden Teilnehmer führt man das Skript durch. Dabei sollte jeder Teilnehmer am AdHoc Netzwerk eine eigene IP-Adresse erhalten. Die letzte Zeile sollte also für jeden Teilnehmer angepasst werden.
-
-Nun kann man mit \lstinline{crontab} (\url{https://wiki.ubuntuusers.de/Cron/}) einstellen, dass das Skript jedes Mal direkt beim Neustart (vor dem Login-Vorgang) ausgeführt wird. Dies ist insbesondere dann hilfreich, wenn das ODROID auf dem Auto montiert ist und keine Tastatur angeschlossen ist.
-Dazu muss man nun \lstinline{crontab -e} ausführen und die folgende Zeile einfügen: \lstinline{@reboot /root/connectAdHoc.sh}. Eventuell ist noch folgender Schritt nötig, um crond automatisch beim Hochfahren zu starten: \lstinline{update-rc.d cron defaults}.
-
-Falls das AdHoc Netzwerk steht, kann man sich mittels \lstinline{ssh root@10.42.43.1} mit dem ODROID verbinden.
-Bei uns kam es vor, dass das AdHoc Netzwerk gelegentlich abgestürzt ist.
-In \lstinline{crontab -e} kann man dann noch folgende Zeile einfügen, um das Adhoc Netzwerk alle 2 Minuten neuzustarten bzw., das entsprchende Skript auszuführen: \lstinline{*/2 * * * * /root/connectAdHoc.sh}.
-\end{document}
diff --git a/modules/CarConfig/environment.config b/modules/CarConfig/kalman.config
similarity index 63%
rename from modules/CarConfig/environment.config
rename to modules/CarConfig/kalman.config
index 9ee0d91d01dfc35bc4b7bcb70f095a527abc65e3..42850675df900bcc09d9aac6ce4f50bbd1cfcbed 100644
--- a/modules/CarConfig/environment.config
+++ b/modules/CarConfig/kalman.config
@@ -1,20 +1,21 @@
// Numbers on name representing indicies
// current car state
-x1: 0
-x2: 0
+x 2 1
+0
+0
// covariance matrix
-p11: 10
-p22: 10
+P 2 2
+10 0
+0 10
// dynamic model
-f11: 1
-f12: 0.001
-f21: 0
-f22: 1
// process error
-q11: 1
-q22: 1
+Q 2 2
+1 0
+0 1
// measure vector
-h1: 1
-h2: 0
+H 2 1
+1
+0
// measurement error
-r: 5
+R 1 1
+5
diff --git a/modules/build.sh b/modules/build.sh
index 0426813b1e3c1553664ac0cda70a2e01ad5a44c8..7716bfdc738dd85b9f4689686205ce3f923c7f43 100755
--- a/modules/build.sh
+++ b/modules/build.sh
@@ -4,7 +4,7 @@
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
DEST=$DIR/CarConfig
LINK=$HOME/CarConfig
-if [ ! -L $LINK ]; then
+if [ ! -e $LINK ]; then
echo "ln -s $DEST $HOME"
ln -s $DEST $HOME
fi
diff --git a/modules/catkin_ws/src/CMakeLists.txt b/modules/catkin_ws/src/CMakeLists.txt
index 581e61db89fce59006b1ceb2d208d9f3e5fbcb5e..3421623bd042f7eb4d2675c8c9ca624feb01cd4f 120000
--- a/modules/catkin_ws/src/CMakeLists.txt
+++ b/modules/catkin_ws/src/CMakeLists.txt
@@ -1 +1 @@
-/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
+/opt/ros/lunar/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt b/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
index 0c7cfd571592fffd5413c021a27271d854bcbd0b..3fbe5601dde854cdf4a006bd0f800f0766af3ac1 100644
--- a/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/NetworkingLib/CMakeLists.txt
@@ -104,24 +104,9 @@ if(INSTALL_PACKAGE)
####################################################
# Create the *Config.cmake and *ConfigVersion files
####################################################
- # Add all targets to the build-tree export set
- export(TARGETS NetworkingLib
- FILE "${PROJECT_BINARY_DIR}/NetworkingLibTargets.cmake")
-
- # Export the package for use from the build-tree
- # (this registers the build-tree with a global CMake-registry)
- export(PACKAGE NetworkingLib)
-
# Get the relative path from the cmake install directory to the include install directory
file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${INSTALL_INCLUDE_DIR}")
- # Configure *Config.cmake.in for the build tree
- # Put it inside the binary directory
- set(CONF_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/include")
- configure_file(NetworkingLibConfig.cmake.in
- "${PROJECT_BINARY_DIR}/NetworkingLibConfig.cmake"
- @ONLY)
-
# Configure *Config.cmake.in for the install tree
# Put it inside the cmake-files directory (which is located inside the binary directory)
set(CONF_INCLUDE_DIRS "\${NETWORKINGLIB_CMAKE_DIR}/${REL_INCLUDE_DIR}/..")
diff --git a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Socket.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Socket.h
index c61e01821f180377cb76ada52700836e9580297e..1436d282c419e061faf659b98e0a8f8ee9416433 100644
--- a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Socket.h
+++ b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Socket.h
@@ -128,7 +128,7 @@ void sendTo(Networking & net,
udp::endpoint endpoint{address::from_string(host), port};
using namespace networking::internal;
- Frame buffer{(const std::uint8_t *) sendData.c_str(), sendData.size()};
+ Frame buffer{(const std::uint8_t *) sendData.c_str(), (std::uint32_t) sendData.size()};
auto asyncOperation = [&socket](auto && ... args)
{ socket.async_send_to(std::forward<decltype(args)>(args)...); };
diff --git a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Stream.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Stream.h
index 2088208bf029e8ab1f335a6c169c0cc65892735d..74e7a988d2623af1a6f977bf2b3cd2edc9ebbccd 100644
--- a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Stream.h
+++ b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/Stream.h
@@ -41,7 +41,7 @@ void write(Networking & net,
const time::Duration & timeout)
{
using namespace networking::internal;
- Frame frame{(const std::uint8_t *) writeData.c_str(), writeData.size()};
+ Frame frame{(const std::uint8_t *) writeData.c_str(), (std::uint32_t) writeData.size()};
auto asyncOperation = [](auto && ... args)
{ boost::asio::async_write(std::forward<decltype(args)>(args)...); };
diff --git a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
index 10cbe19b50a0c94b744cd435b17cd7507436c95b..2107a0b2d9968290f78e86f8d7b3c2e4566102b5 100644
--- a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
+++ b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
@@ -23,29 +23,34 @@ public:
: timestamp(Timestamp::min())
{}
- TimedValue(const T & val)
+ explicit TimedValue(const T & val)
{ set(val); }
TimedValue(const T & val, const Timestamp & timestamp)
- : val(val), timestamp(timestamp)
- {}
-
- T get() const
- { return val; }
+ { set(val, timestamp); }
- void set(const T & val)
+ void set(const T & val, Timestamp timestamp)
{
this->val = val;
- timestamp = networking::time::now();
+ timestamp = timestamp;
}
- Timestamp getTimestamp() const
- { return timestamp; }
+ void set(const T & val)
+ { set(val, networking::time::now()); }
+
+ void set(const TimedValue & val)
+ { set(val.get(), val.getTimestamp()); }
TimedValue & operator=(const T & val)
{ set(val); }
- explicit operator T() const
+ T get() const noexcept
+ { return val; }
+
+ Timestamp getTimestamp() const noexcept
+ { return timestamp; }
+
+ explicit operator T() const noexcept
{ return get(); }
private:
@@ -63,20 +68,38 @@ public:
: timestamp(Timestamp::min())
{}
- TimedAtomicValue(const T & val)
+ explicit TimedAtomicValue(const T & val)
{ set(val); }
- T get()
+ TimedAtomicValue(const T & val, Timestamp timestamp)
+ { set(val, timestamp); }
+
+ explicit TimedAtomicValue(const TimedValue<T> val)
+ { set(val); }
+
+ void set(const T & val, Timestamp timestamp)
{
std::lock_guard <std::mutex> lock{mutex};
- return val;
+ this->val = val;
+ timestamp = timestamp;
}
void set(const T & val)
+ { set(val, networking::time::now()); }
+
+ void set(const TimedValue<T> & val)
+ { set(val.get(), val.getTimestamp()); }
+
+ TimedAtomicValue & operator=(const T & val)
+ { set(val); }
+
+ TimedAtomicValue & operator=(const TimedValue<T> & val)
+ { set(val); }
+
+ T get()
{
std::lock_guard <std::mutex> lock{mutex};
- this->val = val;
- timestamp = networking::time::now();
+ return val;
}
Timestamp getTimestamp()
@@ -85,9 +108,6 @@ public:
return timestamp;
}
- TimedAtomicValue & operator=(const T & val)
- { set(val); }
-
explicit operator T()
{ return get(); }
diff --git a/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt b/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
index 27330da6fc7a025afd26a9eb0436ea0109b2019e..f2b610864228e4bb2d07b7615ca153e7ae985faf 100644
--- a/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
@@ -50,11 +50,6 @@ find_package(NetworkingLib REQUIRED)
target_include_directories(PC2CarLib PUBLIC ${NetworkingLib_INCLUDE_DIRS})
target_link_libraries(PC2CarLib NetworkingLib)
-# PlatoonProtocolLib
-find_package(PlatoonProtocolLib REQUIRED)
-target_include_directories(PC2CarLib PUBLIC ${PlatoonProtocolLib_INCLUDE_DIRS})
-target_link_libraries(PC2CarLib PlatoonProtocolLib)
-
find_package(Boost REQUIRED COMPONENTS system regex)
link_libraries(${Boost_LIBRARIES})
@@ -79,24 +74,9 @@ if(INSTALL_PACKAGE)
####################################################
# Create the *Config.cmake and *ConfigVersion files
####################################################
- # Add all targets to the build-tree export set
- export(TARGETS PC2CarLib
- FILE "${PROJECT_BINARY_DIR}/PC2CarLibTargets.cmake")
-
- # Export the package for use from the build-tree
- # (this registers the build-tree with a global CMake-registry)
- export(PACKAGE PC2CarLib)
-
# Get the relative path from the cmake install directory to the include install directory
file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${INSTALL_INCLUDE_DIR}")
- # Configure *Config.cmake.in for the build tree
- # Put it inside the binary directory
- set(CONF_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/include")
- configure_file(PC2CarLibConfig.cmake.in
- "${PROJECT_BINARY_DIR}/PC2CarLibConfig.cmake"
- @ONLY)
-
# Configure *Config.cmake.in for the install tree
# Put it inside the cmake-files directory (which is located inside the binary directory)
set(CONF_INCLUDE_DIRS "\${PC2CARLIB_CMAKE_DIR}/${REL_INCLUDE_DIR}/..")
@@ -141,9 +121,5 @@ add_executable(PC2CarTest ${TEST_SOURCE_FILES})
target_include_directories(PC2CarTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
target_link_libraries(PC2CarTest NetworkingLib)
-# PlatoonProtocolLib
-target_include_directories(PC2CarTest PUBLIC ${PlatoonProtocolLib_INCLUDE_DIRS})
-target_link_libraries(PC2CarTest PlatoonProtocolLib)
-
# For debugging
target_compile_options(PC2CarTest PUBLIC -fopenmp -fPIC -O0 -g3 -ggdb)
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
index 11e79217613e7301f804475bf1b1e3796a216de7..c2790e6f22d2d1a14241e01030bf05b10bd6ff68 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
@@ -91,24 +91,9 @@ if(${INSTALL_PACKAGE})
####################################################
# Create the *Config.cmake and *ConfigVersion files
####################################################
- # Add all targets to the build-tree export set
- export(TARGETS PlatoonProtocolLib
- FILE "${PROJECT_BINARY_DIR}/PlatoonProtocolLibTargets.cmake")
-
- # Export the package for use from the build-tree
- # (this registers the build-tree with a global CMake-registry)
- export(PACKAGE PlatoonProtocolLib)
-
# Get the relative path from the cmake install directory to the include install directory
file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${INSTALL_INCLUDE_DIR}")
- # Configure *Config.cmake.in for the build tree
- # Put it inside the binary directory
- set(CONF_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/include")
- configure_file(PlatoonProtocolLibConfig.cmake.in
- "${PROJECT_BINARY_DIR}/PlatoonProtocolLibConfig.cmake"
- @ONLY)
-
# Configure *Config.cmake.in for the install tree
# Put it inside the cmake-files directory (which is located inside the binary directory)
set(CONF_INCLUDE_DIRS "\${PLATOON_PROTOCOL_LIB_CMAKE_DIR}/${REL_INCLUDE_DIR}/..")
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
index 6008663a41564198133a8d7b8e46a286c497ded2..247fa3993545102284d0ff7e8e7234021fd8b529 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
@@ -37,7 +37,7 @@ public:
void stop() override;
- PlatoonConfig getPlatoonConfig() const noexcept;
+ networking::time::TimedValue<PlatoonConfig> getPlatoonConfig();
void setOnPlatoonConfigUpdatedCallback(const Callback & callback)
{ onPlatoonConfigUpdatedCallback = callback; }
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
index 6da9b3a89a2a7b3bf057be77571d3333be8d2667..ce5d6a1ad9357c325ae5d23afdb988e9dc9fa034 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
@@ -12,6 +12,7 @@
#include "NetworkingLib/ServiceServer.h"
#include "NetworkingLib/ServiceClient.h"
#include "NetworkingLib/Resolver.h"
+#include "NetworkingLib/TimedValue.h"
#include "NetworkInfo.h"
#include "PlatoonMessage.h"
@@ -76,7 +77,7 @@ protected:
const NetworkInfo myInfo;
PlatoonId platoonId;
- std::atomic<PlatoonConfig> platoonConfig{DEFAULT_PLATOON_CONFIG};
+ networking::time::TimedAtomicValue<PlatoonConfig> platoonConfig{DEFAULT_PLATOON_CONFIG};
State state{State::IDLE};
std::mutex stateMutex;
networking::Networking & net;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
index 816c297a413c54ebed574b4bac9af14be095161f..a3396c1ec108bc71f8e915c3e5f1b52ea8cdacb2 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
@@ -50,7 +50,7 @@ public:
// FollowerVehicle methods
- PlatoonConfig getPlatoonConfig();
+ networking::time::TimedValue<PlatoonConfig> getPlatoonConfig();
private:
Vehicle::Ptr vehicle;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
index e30ac786fa63d53429d888b366b0d3e47078e949..74adffec4971a8b31646d82f36f9e57b80189687 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
@@ -35,9 +35,9 @@ void FollowerVehicle::stop()
stopBroadcastTimer();
}
-PlatoonConfig FollowerVehicle::getPlatoonConfig() const noexcept
+networking::time::TimedValue<PlatoonConfig> FollowerVehicle::getPlatoonConfig()
{
- return platoonConfig.load();
+ return platoonConfig.getNonAtomicCopy();
}
void FollowerVehicle::doCreatePlatoon()
@@ -165,7 +165,7 @@ void FollowerVehicle::receiveBroadcast(const PlatoonMessage & broadcast)
startBroadcastTimer();
PlatoonConfig newConfig{broadcast.platoonSpeed, broadcast.innerPlatoonDistance};
- if (platoonConfig.load() != newConfig)
+ if (platoonConfig.get() != newConfig)
{
platoonConfig = newConfig;
onPlatoonConfigUpdatedCallback();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
index a908df36624545a834fa07b938efbd1cf556e45a..e485fb4bd1551ec0786cc88438f4e7f4343b2760 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
@@ -40,12 +40,12 @@ void LeaderVehicle::setPlatoonConfig(const PlatoonConfig & platoonConfig)
void LeaderVehicle::setPlatoonSpeed(PlatoonSpeed platoonSpeed)
{
- platoonConfig = PlatoonConfig{platoonSpeed, platoonConfig.load().innerPlatoonDistance};
+ platoonConfig = PlatoonConfig{platoonSpeed, platoonConfig.get().innerPlatoonDistance};
}
void LeaderVehicle::setInnerPlatoonDistance(InnerPlatoonDistance innerPlatoonDistance)
{
- platoonConfig = PlatoonConfig{platoonConfig.load().platoonSpeed, innerPlatoonDistance};
+ platoonConfig = PlatoonConfig{platoonConfig.get().platoonSpeed, innerPlatoonDistance};
}
std::size_t LeaderVehicle::getNumFollowers()
@@ -215,7 +215,7 @@ void LeaderVehicle::stopBroadcasting()
void LeaderVehicle::sendBroadcastMessage()
{
- PlatoonConfig configCopy = platoonConfig.load();
+ PlatoonConfig configCopy = platoonConfig.get();
// Send the broadcast message and forget about it.
sendMessage(PlatoonMessage::broadcastMessage(
myInfo.vehicleId,
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
index db4ea64ec269ec7f5181514153d740aefa12f498..f04d83c7751bc519f6fc3256796f8f4604f6c4fb 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
@@ -111,7 +111,7 @@ void VehicleFacade::setInnerPlatoonDistance(InnerPlatoonDistance innerPlatoonDis
std::dynamic_pointer_cast<LeaderVehicle>(vehicle)->setInnerPlatoonDistance(innerPlatoonDistance);
}
-PlatoonConfig VehicleFacade::getPlatoonConfig()
+networking::time::TimedValue<PlatoonConfig> VehicleFacade::getPlatoonConfig()
{
std::lock_guard<std::mutex> lock(mutex);
return std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->getPlatoonConfig();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
index 06c38c0f690ff31ecdfeb60ad563532cad391db7..d3b73d395cf604c9951c65bf4f20e66ff66c3a66 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
@@ -30,8 +30,8 @@ int main(int argc, char ** argv)
[follower]
{
auto config = follower->getPlatoonConfig();
- std::cout << "Config Update:\nIPD=" << config.innerPlatoonDistance
- << "\nPS=" << config.platoonSpeed << "\n";
+ std::cout << "Config Update:\nIPD=" << config.get().innerPlatoonDistance
+ << "\nPS=" << config.get().platoonSpeed << "\n";
});
follower->createPlatoon();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
index 9fe2c35c785161d2cf85ed60ed09578d31e5cfc9..763b2b1539876b53edfbb4951f6c247bcb510367 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
@@ -78,7 +78,7 @@ bool test2Follower1()
return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
{
return follower1->isPlatoonRunning() &&
- follower1->getPlatoonConfig() == TEST_CONFIG;
+ follower1->getPlatoonConfig().get() == TEST_CONFIG;
});
}
@@ -126,7 +126,7 @@ bool test3Follower1()
auto deltaTime = now - lastConfigTime;
lastConfigTime = now;
- if (follower1->getPlatoonConfig() != expectedConfig)
+ if (follower1->getPlatoonConfig().get() != expectedConfig)
{
success = false;
running = false;
@@ -293,8 +293,8 @@ bool test8Followers(FollowerVehicle::Ptr follower)
{
auto delta = networking::time::now() - last;
std::cout << "time passed [ms]: " << std::chrono::duration_cast<std::chrono::milliseconds>(delta).count() << "\n";
- std::cout << "ps: " << follower->getPlatoonConfig().platoonSpeed << "\n";
- if (follower->getPlatoonConfig() != expectedConfig)
+ std::cout << "ps: " << follower->getPlatoonConfig().get().platoonSpeed << "\n";
+ if (follower->getPlatoonConfig().get() != expectedConfig)
{
success = false;
return;
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt b/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt
index f3edb8b1ffaf658b77f2a4429c2385d987aa64a7..7fae0258436d90ccdcf4b6f77a48b5f9664d533c 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/VeloxProtocolLib/CMakeLists.txt
@@ -106,24 +106,9 @@ if(INSTALL_PACKAGE)
####################################################
# Create the *Config.cmake and *ConfigVersion files
####################################################
- # Add all targets to the build-tree export set
- export(TARGETS VeloxProtocolLib
- FILE "${PROJECT_BINARY_DIR}/VeloxProtocolLibTargets.cmake")
-
- # Export the package for use from the build-tree
- # (this registers the build-tree with a global CMake-registry)
- export(PACKAGE VeloxProtocolLib)
-
# Get the relative path from the cmake install directory to the include install directory
file(RELATIVE_PATH REL_INCLUDE_DIR "${INSTALL_CMAKE_DIR}" "${INSTALL_INCLUDE_DIR}")
- # Configure *Config.cmake.in for the build tree
- # Put it inside the binary directory
- set(CONF_INCLUDE_DIRS "${CMAKE_CURRENT_LIST_DIR}/include")
- configure_file(VeloxProtocolLibConfig.cmake.in
- "${PROJECT_BINARY_DIR}/VeloxProtocolLibConfig.cmake"
- @ONLY)
-
# Configure *Config.cmake.in for the install tree
# Put it inside the cmake-files directory (which is located inside the binary directory)
set(CONF_INCLUDE_DIRS "\${VELOX_PROTOCOL_LIB_CMAKE_DIR}/${REL_INCLUDE_DIR}/..")
@@ -163,8 +148,8 @@ endif()
#######
# Test
#######
-add_executable(RemoteControl ${SOURCE_FILES} test/RemoteControl.cpp)
+add_executable(TerminalControl ${SOURCE_FILES} test/TerminalControl.cpp)
# NetworkingLib
-target_include_directories(RemoteControl PUBLIC ${NetworkingLib_INCLUDE_DIRS})
-target_link_libraries(RemoteControl NetworkingLib)
\ No newline at end of file
+target_include_directories(TerminalControl PUBLIC ${NetworkingLib_INCLUDE_DIRS})
+target_link_libraries(TerminalControl NetworkingLib)
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/VeloxProtocolLibConfig.cmake.in b/modules/catkin_ws/src/VeloxProtocolLib/VeloxProtocolLibConfig.cmake.in
index 00b34039f0d04ff212d972020c6dbaebfdd5c7cf..3dde9331773fc2ab2a8f904e46cfea6820ab4cf9 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/VeloxProtocolLibConfig.cmake.in
+++ b/modules/catkin_ws/src/VeloxProtocolLib/VeloxProtocolLibConfig.cmake.in
@@ -3,7 +3,7 @@
# VeloxProtocolLib_INCLUDE_DIRS - include directories for VeloxProtocolLib
# Compute paths
-get_filename_component(NETWORKINGLIB_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
+get_filename_component(VELOX_PROTOCOL_LIB_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH)
set(VeloxProtocolLib_INCLUDE_DIRS "@CONF_INCLUDE_DIRS@")
# Our library dependencies (contains definitions for IMPORTED targets)
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h b/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
index b176cfc1aa260cd0128550a457f88f0c875c5ab6..62889e79f72eb52712217f67ab5ad8c39898fff2 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
@@ -75,8 +75,6 @@ private:
networking::BusyLock busyLock;
Connection::Ptr self;
-
- void closeOnce();
};
networking::Networking & net;
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/test/RemoteControl.cpp b/modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
similarity index 100%
rename from modules/catkin_ws/src/VeloxProtocolLib/test/RemoteControl.cpp
rename to modules/catkin_ws/src/VeloxProtocolLib/test/TerminalControl.cpp
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 7cbb63f0356fb975cad43bc12aa17d93307ccaf7..97c414434b9b577bc524979a16dc30f80adecdef 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -51,6 +51,21 @@ include_directories(
)
set(MAIN_NODE_SOURCE_FILES
+ include/camera/Camera.h
+ include/environment/Environment.h
+ include/exceptions/Exceptions.h
+ include/lanekeeping/Lanekeeping.h
+ include/logging/Logging.h
+ include/mainNode/CruiseController.h
+ include/mainNode/EgoMotion.h
+ include/mainNode/MainNode.h
+ include/mainNode/NotifiableThread.h
+ include/mainNode/PlatoonController.h
+ include/mainNode/PlatoonState.h
+ include/mavLink/MavLink.h
+ include/ultrasonic/StreamMedianFilter.h
+ include/ultrasonic/Ultrasonic.h
+ include/ultrasonic/USS_SRF02.h
src/mainNode/MainNode.cpp
src/mainNode/EgoMotion.cpp
src/mainNode/NotifiableThread.cpp
@@ -58,6 +73,11 @@ set(MAIN_NODE_SOURCE_FILES
src/mainNode/CruiseController.cpp
)
+set(ENVIRONMENT_SOURCE_FILES
+ src/environment/Environment.cpp
+ src/environment/KalmanFilter.cpp
+)
+
set(USS_SOURCE_FILES
src/ultrasonic/StreamMedianFilter.cpp
src/ultrasonic/Ultrasonic.cpp
@@ -71,6 +91,7 @@ set(CMAKE_PREFIX_PATH "${LOCAL_INSTALL_DIR}")
find_package(NetworkingLib REQUIRED)
find_package(PlatoonProtocolLib REQUIRED)
find_package(PC2CarLib REQUIRED)
+find_package(VeloxProtocolLib REQUIRED)
add_library(main_node ${MAIN_NODE_SOURCE_FILES})
add_dependencies(main_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
@@ -85,7 +106,7 @@ target_include_directories(main_node PUBLIC
${PlatoonProtocolLib_INCLUDE_DIRS}
${PC2CarLib_INCLUDE_DIRS})
-add_library(environment src/environment/Environment.cpp)
+add_library(environment ${ENVIRONMENT_SOURCE_FILES})
add_dependencies(environment ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(environment ${catkin_LIBRARIES})
@@ -109,7 +130,11 @@ target_link_libraries(lanekeeping ${catkin_LIBRARIES})
add_library(mav_link src/mavLink/MavLink.cpp)
add_dependencies(mav_link ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
-target_link_libraries(mav_link ${catkin_LIBRARIES})
+target_link_libraries(mav_link
+ ${catkin_LIBRARIES}
+ VeloxProtocolLib)
+target_include_directories(mav_link PUBLIC
+ ${VeloxProtocolLib_INCLUDE_DIRS})
# NotifiableThread Test
set(NOTIFIABLE_THREAD_TEST_SOURCE_FILES
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index c869343c35a1f44b2a295c205fa19150b55e1db6..555eecbd66cf8822ed83f54d120bac7f7cbbd859 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -26,7 +26,6 @@ public:
private:
ros::NodeHandle nh_;
std::string name_;
- void readConfigFile();
float distance;
float relativeSpeed;
@@ -38,16 +37,7 @@ private:
void ussDataCallback(const ussDataMsg::ConstPtr & inMsg);
void camDataCallback(const camDataMsg::ConstPtr & inMsg);
- //Notation like in Kalamanfilter <Wikipedia>
- vector<double> x {2}; // current state (distance, velocity)^T
- matrix<double> P {2, 2}; // covariance matrix
- matrix<double> F {2, 2}; // dynamic model
- matrix<double> Q {2, 2}; // process error
- vector<double> H {2}; // mesure vector (refers to distance)
- vector<double> R {1}; // measurement error
-
- void predict();
- void update(vector<double> mesVec);
+
};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/environment/KalmanFilter.h b/modules/catkin_ws/src/car/include/environment/KalmanFilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..08ca6427e59b9cb025b7bc84ce17298099d89265
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/environment/KalmanFilter.h
@@ -0,0 +1,43 @@
+#ifndef KALMANFILTER_H
+#define KALMANFILTER_H
+
+#include <boost/numeric/ublas/vector.hpp>
+#include <boost/numeric/ublas/matrix.hpp>
+#include <vector>
+#include <fstream>
+
+using namespace boost::numeric::ublas;
+namespace car
+{
+class KalmanFilter
+{
+public:
+ KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT);
+
+ KalmanFilter(int dimension, matrix<double> F, double *deltaT, std::string configFilePath);
+
+ ~KalmanFilter();
+
+ vector<double> nextKalmanIteration(vector<double> mesVec);
+
+private:
+ //Notation like in Kalamanfilter <Wikipedia>
+ vector<double> x; // current state (distance, velocity)^T
+ matrix<double> P; // covariance matrix
+ matrix<double> F; // dynamic model
+ matrix<double> Q; // process error
+ vector<double> H; // mesure vector (refers to distance)
+ vector<double> R; // measurement error
+
+ double *dt; // delta t
+
+ std::vector<std::string> getNextFileLine(std::ifstream *file);
+
+ void readConfigFile(std::string configFilePath);
+
+ void predict();
+
+ void update(vector<double> mesVec);
+};
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
index 9446809bd234e8f7692655aaffceea9d54ee7c20..c94ccd825eada38afff9c9a3bce7d8fc69f7a4e7 100644
--- a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
+++ b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
@@ -9,21 +9,25 @@
namespace car
{
- class Lanekeeping : public nodelet::Nodelet
- {
- public:
- virtual void onInit();
- Lanekeeping(ros::NodeHandle &nh, std::string &name);
- Lanekeeping();
- ~Lanekeeping();
- private:
- ros::NodeHandle nh_;
- std::string name_;
-
- ros::Publisher laneData;
- ros::Subscriber camData;
-
- void camDataCallback(const camDataMsg::ConstPtr& inMsg);
- };
+class Lanekeeping : public nodelet::Nodelet
+{
+public:
+ virtual void onInit();
+
+ Lanekeeping(ros::NodeHandle & nh, std::string & name);
+
+ Lanekeeping();
+
+ ~Lanekeeping();
+
+private:
+ ros::NodeHandle nh_;
+ std::string name_;
+
+ ros::Publisher laneData;
+ ros::Subscriber camData;
+
+ void camDataCallback(const camDataMsg::ConstPtr & inMsg);
+};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
index a94dcb7830087835635a5e6a7dbfdd3ec0ad09e6..c2f1644437a1578bec72b906510f1a1d370198d8 100644
--- a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -1,9 +1,12 @@
#ifndef CRUISECONTROLLER_H
#define CRUISECONTROLLER_H
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
#include "PlatoonProtocolLib/VehicleFacade.h"
#include "PlatoonController.h"
#include "EgoMotion.h"
+#include "PlatoonState.h"
namespace car
{
@@ -12,7 +15,8 @@ class CruiseController
public:
CruiseController(platoonProtocol::VehicleFacade& c2c,
EgoMotion& egoMotion,
- PlatoonController& platoonController);
+ PlatoonController& platoonController,
+ ros::NodeHandle & nh);
~CruiseController(){};
@@ -22,6 +26,23 @@ class CruiseController
platoonProtocol::VehicleFacade& c2c;
EgoMotion& egoMotion;
PlatoonController& platoonController;
+
+ float min_speed{0};
+ float max_speed{20};
+ float crash_time{5};
+ float stop_dist{20};
+
+ float eps{1e-9};
+ float speed{0};
+ float units_conv_fac{1};
+
+ float dist_pow {1.5};
+ float speed_pow{0.5};
+
+ void run_ACC(float target_speed);
+ void run_CACC();
+
+ ros::Publisher ccData;
};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index e4d28b13aaf2da0d3ec7225f313fb01c41d231fa..785eec3e23b42deb37ec980cf3a6ef2206b66b3f 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -24,6 +24,8 @@ class EgoMotion
Callback cruiseControllerNotify;
float getDistance(){return distance;}
+ float getPrvSpeed(){return prvSpeed;}
+ float getOwnSpeed(){return stmSpeed;}
private:
std::atomic<float> distance{0};
@@ -38,7 +40,6 @@ class EgoMotion
void stmDataCallback(const stmDataMsg::ConstPtr & inMsg);
void computeAndNotify();
- float getPrvSpeed(){return prvSpeed;}
};
}
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index 442508b00da25ca85a83af878bc801052688543f..412105186db157694ba275c02bfb785e2f670845 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -22,44 +22,51 @@ class PlatoonController
{
public:
using Callback = std::function<void()>;
-
- PlatoonController(platoonProtocol::VehicleFacade& c2c,
+
+ PlatoonController(platoonProtocol::VehicleFacade & c2c,
pc2car::CommandReceiver::Ptr pc,
- EgoMotion& egoMotion);
-
+ EgoMotion & egoMotion);
+
void run();
+
+ float getDesSpeed() { return desSpeed.get(); }
Callback cruiseControllerNotify;
-
- // TODO make values ATOMIC !
- // these value need to be stored and atomic, since they will be pulled from other modules
- PlatoonState curState = PlatoonState::ACC;
-
- platoonProtocol::PlatoonConfig platoonConfig; // TODO needs to be removed, once C2C returns TimedValues
+
+ PlatoonState getCurrState() const
+ { return currState; }
+
+ networking::time::TimedValue<platoonProtocol::PlatoonConfig> getPlatoonConfig()
+ { return platoonConfig.getNonAtomicCopy(); }
private:
- platoonProtocol::VehicleFacade& c2c;
+ platoonProtocol::VehicleFacade & c2c;
pc2car::CommandReceiver::Ptr pc;
- EgoMotion& egoMotion;
-
+ EgoMotion & egoMotion;
+ std::atomic<PlatoonState> currState{PlatoonState::ACC};
+ networking::time::TimedAtomicValue<platoonProtocol::PlatoonConfig> platoonConfig;
bool c2cAlive = false;
// void updateC2cConfig(); // once C2C returns TimedValues
-
+
void updatePcConfig();
- pc2car::TimedValue<platoonProtocol::PlatoonSpeed> PS{0.0f};
- pc2car::TimedValue<platoonProtocol::InnerPlatoonDistance> IPD{0.0f};
-
+
+ networking::time::TimedValue<platoonProtocol::PlatoonSpeed> PS{0.0f};
+ networking::time::TimedValue<platoonProtocol::InnerPlatoonDistance> IPD{0.0f};
+
void updateDesSpeed();
- pc2car::TimedValue<float> desSpeed{0.0f};
-
+
+ networking::time::TimedValue<float> desSpeed{0.0f};
+
void setupC2C();
-
+
// METHODS
void run_ACC();
+
void run_CACC_FV();
+
void run_CACC_LV();
-
+
};
}
diff --git a/modules/catkin_ws/src/car/include/mavLink/MavLink.h b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
index 857c542dea73edcfce69bbe7b738b6596b91cde1..63543688a278db9677c4c645e026a6a98779df4a 100644
--- a/modules/catkin_ws/src/car/include/mavLink/MavLink.h
+++ b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
@@ -9,32 +9,46 @@
#include "car/rcEnabledMsg.h"
#include "boost/thread.hpp"
+#include "NetworkingLib/Networking.h"
+#include "VeloxProtocolLib/Connection.h"
namespace car
{
- class MavLink : public nodelet::Nodelet
- {
- public:
- virtual void onInit();
- MavLink(ros::NodeHandle &nh, std::string &name);
- MavLink();
- ~MavLink();
- private:
- ros::NodeHandle nh_;
- std::string name_;
-
- ros::Publisher stmData;
- ros::Subscriber ccData;
- ros::Subscriber laneData;
- ros::Subscriber rcEnabled;
-
-
- void ccDataCallback(const ccDataMsg::ConstPtr& inMsg);
- void laneDataCallback(const laneDataMsg::ConstPtr& inMsg);
- void rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg);
-
-
- boost::thread main;
- };
+class MavLink : public nodelet::Nodelet
+{
+public:
+ virtual void onInit();
+
+ MavLink(ros::NodeHandle & nh, std::string & name);
+
+ MavLink();
+
+ ~MavLink();
+
+private:
+ ros::NodeHandle nh_;
+ std::string name_;
+
+ ros::Publisher stmData;
+ ros::Subscriber ccData;
+ ros::Subscriber laneData;
+ ros::Subscriber rcEnabled;
+
+ void ccDataCallback(const ccDataMsg::ConstPtr & inMsg);
+
+ void laneDataCallback(const laneDataMsg::ConstPtr & inMsg);
+
+ void rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg);
+
+ void onStmDataReceived();
+
+ float convertSpeedToStm(float speed) const;
+
+ float convertSpeedFromStm(float speed) const;
+
+ networking::Networking net;
+ veloxProtocol::Connection::Ptr veloxConnection;
+};
+
}
#endif
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index 7fb8a84c5386518fd0c0104b94c2ad10c4fd6acb..3fe8de926f5eecbed5832e2115314f4f3ae875a0 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -8,10 +8,6 @@
#include "car/environmentDataMsg.h"
#include "car/ussDataMsg.h"
-#include <fstream>
-#include <vector>
-#include <iterator>
-
PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
namespace car
@@ -21,82 +17,15 @@ Environment::Environment(ros::NodeHandle &nh, std::string &name) :
, name_(name)
, distance(0)
, relativeSpeed(0)
-{
- readConfigFile();
-}
+ {}
Environment::Environment() :
distance(0)
, relativeSpeed(0)
-{
- readConfigFile();
-}
+ {}
Environment::~Environment() {}
-void Environment::readConfigFile()
-{
- // open config file
- std::string userHome = getenv("HOME");
- std::ifstream configFile;
- configFile.open(userHome + "/CarConfig/environment.config", std::ifstream::in);
- if (!configFile.is_open()) { throw FileNotFound(); }
-
- // desired parameters
-
- // read file
- std::string contentLine;
- while (!configFile.eof())
- {
- std::getline(configFile, contentLine);
-
- // split this line
- std::istringstream iss(contentLine);
- std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
-
- if ( words.size() > 1 ) // this line contains parameter[at (0)] and value [at (1)]
- {
- if (words.at(0) == "x1:") {
- x(0) = std::stod(words.at(1));
- } else if (words.at(0) == "x2:"){
- x(1) = std::stof(words.at(1));
- } else if (words.at(0) == "p11:"){
- P(0, 0) = std::stof(words.at(1));
- } else if (words.at(0) == "p12:"){
- P(0,1) = std::stof(words.at(1));
- } else if (words.at(0) == "p21:"){
- P(1,0) = std::stof(words.at(1));
- } else if (words.at(0) == "p22:"){
- P(1,1) = std::stof(words.at(1));
- } else if (words.at(0) == "f11:"){
- F(0,0) = std::stof(words.at(1));
- } else if (words.at(0) == "f12:"){
- F(0,1) = std::stof(words.at(1));
- } else if (words.at(0) == "f21:"){
- F(1,0) = std::stof(words.at(1));
- } else if (words.at(0) == "f22:"){
- F(1,1) = std::stof(words.at(1));
- } else if (words.at(0) == "q11:"){
- Q(0,0) = std::stof(words.at(1));
- } else if (words.at(0) == "q12:"){
- Q(0,1) = std::stof(words.at(1));
- } else if (words.at(0) == "q21:"){
- Q(1,0) = std::stof(words.at(1));
- } else if (words.at(0) == "q22:"){
- Q(1,1) = std::stof(words.at(1));
- } else if (words.at(0) == "h1:"){
- H(0) = std::stof(words.at(1));
- } else if (words.at(0) == "h2:"){
- H(1) = std::stof(words.at(1));
- } else if (words.at(0) == "r:"){
- R(0) = std::stof(words.at(1));
- }
- }
- }
- configFile.close();
- return;
-}
-
void Environment::onInit()
{
NODELET_INFO("Environment::onInit -- START");
diff --git a/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..86c51897263411513c3a0830b313f00b07ea93a8
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/environment/KalmanFilter.cpp
@@ -0,0 +1,131 @@
+#include "environment/KalmanFilter.h"
+#include "exceptions/Exceptions.h"
+#include <iterator>
+
+namespace car
+{
+KalmanFilter::KalmanFilter(vector<double> x, matrix<double> P, matrix<double> F, matrix<double> Q, vector<double> H, vector<double> R, double *deltaT)
+ : x(x)
+ , P(P)
+ , F(F)
+ , Q(Q)
+ , H(H)
+ , R(R)
+ , dt(deltaT)
+ {}
+
+KalmanFilter::KalmanFilter(int dimension, matrix<double> F, double *deltaT, std::string configFilePath)
+ : x(dimension)
+ , P(dimension, dimension)
+ , F(F)
+ , Q(dimension, dimension)
+ , H(dimension)
+ , R(dimension)
+ , dt(deltaT)
+ {
+ readConfigFile(configFilePath);
+ }
+
+KalmanFilter::~KalmanFilter() {}
+
+std::vector<std::string> KalmanFilter::getNextFileLine(std::ifstream *file)
+{
+ // read a line
+ std::string contentLine;
+ std::getline(*file, contentLine);
+
+ // split this line
+ std::istringstream iss(contentLine);
+ std::vector<std::string> words {std::istream_iterator<std::string>(iss), {} };
+ return words;
+}
+
+void KalmanFilter::readConfigFile(std::string configFilePath)
+{
+ // open config file
+ std::string userHome = getenv("HOME");
+ std::ifstream configFile;
+ configFile.open(configFilePath, std::ifstream::in);
+ if (!configFile.is_open()) { throw FileNotFound(); }
+
+ // read file
+ while (!configFile.eof())
+ {
+ std::vector<std::string> words = getNextFileLine(&configFile);
+ if ( words.size() == 3 ) // this line contains parameter[at (0)] and value [at (1)]
+ {
+ if ( words.at(0) == "//") { continue; }
+ int rows = std::stoi(words.at(1));
+ int cols = std::stoi(words.at(2));
+ int i = 0;
+ int j = 0;
+ if (words.at(0) == "x") {
+ while ( i < rows )
+ {
+ words = getNextFileLine(&configFile);
+ x(i) = std::stod(words.at(0));
+ }
+ } else if (words.at(0) == "P"){
+ while ( i < rows )
+ {
+ words = getNextFileLine(&configFile);
+ while ( j < cols )
+ {
+ P(i, j) = std::stod(words.at(j));
+ }
+ }
+ } else if (words.at(0) == "Q"){
+ while ( i < rows )
+ {
+ words = getNextFileLine(&configFile);
+ while ( j < cols )
+ {
+ Q(i, j) = std::stod(words.at(j));
+ }
+ }
+ } else if (words.at(0) == "H"){
+ while ( i < rows )
+ {
+ words = getNextFileLine(&configFile);
+ H(i) = std::stod(words.at(0));
+ }
+ } else if (words.at(0) == "R"){
+ while ( i < rows )
+ {
+ words = getNextFileLine(&configFile);
+ x(i) = std::stod(words.at(j));
+ }
+ }
+ }
+ }
+ configFile.close();
+ return;
+}
+
+void KalmanFilter::predict(){
+ // x = F*x
+ // P = (F * P * F.T) + Q
+}
+
+void KalmanFilter::update(vector<double> mesVec){
+ // Innovation
+ // y = mesVec.T, - H * x
+
+ // Residualkovarianz
+ // S = H * P * H.T + R
+
+ // Kalman - Gain
+ // K = P * H.T * S**(-1)
+
+ // update the predecited values
+ // x = x + K * y
+ // P = P - K * S * K.T
+}
+
+vector<double> KalmanFilter::nextKalmanIteration(vector <double> mesVec)
+{
+ update(mesVec);
+ predict();
+ return x;
+}
+}
diff --git a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
index 6ef021265d0ae9b2cc6b7593d348cfcbc64fc5fa..499be7173cf9149d18137b401e026406d7b77c7a 100644
--- a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
+++ b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
@@ -10,27 +10,28 @@ PLUGINLIB_EXPORT_CLASS(car::Lanekeeping, nodelet::Nodelet);
namespace car
{
- Lanekeeping::Lanekeeping(ros::NodeHandle &nh, std::string &name) :
- nh_(nh),
- name_(name)
- { }
-
- Lanekeeping::Lanekeeping() {}
-
- Lanekeeping::~Lanekeeping() {}
-
- void Lanekeeping::onInit()
- {
- NODELET_INFO("Lanekeeping::onInit -- START");
- laneData = nh_.advertise<laneDataMsg>("laneData", 1);
- camData = nh_.subscribe("camData", 1, &Lanekeeping::camDataCallback, this);
- NODELET_INFO("Lanekeeping::onInit -- END");
- }
-
- void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr& inMsg)
- {
+Lanekeeping::Lanekeeping(ros::NodeHandle & nh, std::string & name) :
+ nh_(nh), name_(name)
+{}
+
+Lanekeeping::Lanekeeping()
+{}
+
+Lanekeeping::~Lanekeeping()
+{}
+
+void Lanekeeping::onInit()
+{
+ NODELET_INFO("Lanekeeping::onInit -- START");
+ laneData = nh_.advertise<laneDataMsg>("laneData", 1);
+ camData = nh_.subscribe("camData", 1, &Lanekeeping::camDataCallback, this);
+ NODELET_INFO("Lanekeeping::onInit -- END");
+}
+
+void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr & inMsg)
+{
// std::cout << "Lanekeeping recived new cam data ( ).\n";
- }
-
-
+}
+
+
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 32651413d484ac9deaa2f26417f1a3d84d7b4439..49dfdb02ca60427d46a9ded875067993b2c17a4a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -1,21 +1,88 @@
#include "mainNode/CruiseController.h"
#include "PlatoonProtocolLib/VehicleFacade.h"
+#include "PlatoonProtocolLib/Protocol.h"
+
+#include "car/ccDataMsg.h"
#include <iostream>
+#include <algorithm>
namespace car {
CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c,
EgoMotion& egoMotion,
- PlatoonController& platoonController)
+ PlatoonController& platoonController,
+ ros::NodeHandle& nh)
: c2c(c2c)
, egoMotion(egoMotion)
, platoonController(platoonController)
-{ }
+ {
+ ccData = nh.advertise<ccDataMsg>("ccData", 1);
+ }
void CruiseController::run() {
- std::cout << "CruiseController was run." << std::endl;
+ switch (platoonController.getCurrState()) {
+ case PlatoonState::ACC: {
+ run_ACC(platoonController.getDesSpeed());
+ break;
+ }
+ case PlatoonState::CACC_LV: {
+ auto config = platoonController.getPlatoonConfig().get();
+ run_ACC(config.platoonSpeed);
+ c2c.setPlatoonSpeed(speed);
+ break;
+ }
+ case PlatoonState::CACC_FV: { run_CACC(); break; }
+ }
+}
+
+void CruiseController::run_ACC(float target_speed) {
+
+ std::cout << "CruiseController::run_ACC("<< target_speed <<")" << std::endl;
+ float v_self = egoMotion.getOwnSpeed();
+ float d = egoMotion.getDistance();
+ float crash_dist = crash_time * v_self;
+
+ if ( d < crash_dist ) {
+ std::cout << "in crash_dist" << std::endl;
+ speed = target_speed*(d - stop_dist)/crash_dist;
+ } else {
+ std::cout << "not in crash_dist" << std::endl;
+ speed = target_speed;
+ }
+
+ speed = std::min(std::max(speed, min_speed), max_speed);
+
+ ccDataMsg outMsg;
+ outMsg.speed = speed;
+ ccData.publish(outMsg);
+}
+
+void CruiseController::run_CACC() {
+
+ std::cout << "CruiseController::run_CACC" << std::endl;
+
+ auto config = platoonController.getPlatoonConfig().get();
+ float PS = config.platoonSpeed;
+ float IPD = config.innerPlatoonDistance;
+ float vp = egoMotion.getPrvSpeed();
+ float d = egoMotion.getDistance();
+
+ if ( IPD < 1 ) { IPD = 1; }
+ if ( vp < eps && vp > -eps ) {
+ if ( d < IPD ) { speed = 0; return; }
+ speed = units_conv_fac*0.5*(d-IPD)/IPD; // THIS IS DEPENDEND ON THE UNITS OF speed AND d!!!
+ } else {
+ speed = pow(d/IPD,dist_pow) * pow(PS/vp,speed_pow) * vp;
+ }
+
+ speed = std::min(std::max(speed, min_speed), max_speed);
+
+ ccDataMsg outMsg;
+ outMsg.speed = speed;
+ ccData.publish(outMsg);
}
+
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 11d44623bd919d81f9ba69a5fca94a6aaeca9482..452ded78ba95fb6aaca99e32a625f05f3c7206a4 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -70,7 +70,7 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController, nh)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
@@ -90,7 +90,7 @@ MainNode::MainNode() :
, egoMotion(nh)
, platoonController(c2c, pc, egoMotion)
, platoonControllerThread([this] { platoonController.run(); })
- , cruiseController(c2c, egoMotion, platoonController)
+ , cruiseController(c2c, egoMotion, platoonController, nh)
, cruiseControllerThread([this] { cruiseController.run(); })
{
egoMotion.cruiseControllerNotify = cruiseControllerNotify;
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index c8702dd04cba6ce428bd8521b3dc13d72221330a..b2d853d7852d7945449894b1d93e198d75f25c00 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -68,23 +68,10 @@ void PlatoonController::setupC2C()
void PlatoonController::run()
{
std::cout << "PlatoonController was run." << std::endl;
- switch (curState)
- {
- case PlatoonState::ACC:
- {
- run_ACC();
- break;
- }
- case PlatoonState::CACC_FV:
- {
- run_CACC_FV();
- break;
- }
- case PlatoonState::CACC_LV:
- {
- run_CACC_LV();
- break;
- }
+ switch (currState.load()) {
+ case PlatoonState::ACC: { run_ACC(); break; }
+ case PlatoonState::CACC_FV: { run_CACC_FV(); break; }
+ case PlatoonState::CACC_LV: { run_CACC_LV(); break; }
}
}
@@ -92,43 +79,33 @@ void PlatoonController::run_ACC()
{
bool wantsPlatoon = pc->isPlatoonEnabled().get();
- if (!wantsPlatoon)
- {
- if (c2cAlive)
- {
- c2c.leavePlatoon();
- c2cAlive = false;
- }
+ if (!wantsPlatoon) {
+ if (c2cAlive) { c2c.leavePlatoon(); c2cAlive = false;}
updateDesSpeed();
return;
- }
-
+ }
+
// wantsPlatoon
- if (!c2cAlive)
- {
+ if (!c2cAlive) {
updateDesSpeed();
setupC2C();
return;
}
// c2cAlive + wantsPlatoon
- if (c2c.isPlatoonRunning())
- {
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
- {
+ if (c2c.isPlatoonRunning()) {
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {
updatePcConfig();
- curState = PlatoonState::CACC_LV;
+ currState = PlatoonState::CACC_LV;
}
- else
- { curState = PlatoonState::CACC_FV; }
+ else {currState = PlatoonState::CACC_FV;}
return;
}
// !inPlatoon + c2cAlive + wantsPlatoon
bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity();
- if (isLeader != hasFiniteDistance)
- { // role does not fit
+ if ( isLeader != hasFiniteDistance ) { // role does not fit
c2c.leavePlatoon();
c2cAlive = false;
updateDesSpeed();
@@ -137,10 +114,8 @@ void PlatoonController::run_ACC()
}
// role fits + !inPlatoon + c2cAlive + wantsPlatoon
- if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
- { updatePcConfig(); }
+ if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {updatePcConfig();}
updateDesSpeed();
- return;
}
void PlatoonController::run_CACC_FV()
@@ -155,20 +130,14 @@ void PlatoonController::run_CACC_FV()
std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
<< ", wantsPlatoon = " << wantsPlatoon << std::endl;
- if (inPlatoon && wantsPlatoon)
- {
- cruiseControllerNotify();
- return;
- }
-
- if (inPlatoon && !wantsPlatoon)
- { c2c.leavePlatoon(); }
-
+ if (inPlatoon && wantsPlatoon) { cruiseControllerNotify(); return; }
+
+ if (inPlatoon && !wantsPlatoon) { c2c.leavePlatoon(); }
+
c2cAlive = false;
- curState = PlatoonState::ACC;
+ currState = PlatoonState::ACC;
cruiseControllerNotify();
run_ACC();
- return;
}
void PlatoonController::run_CACC_LV()
@@ -179,24 +148,20 @@ void PlatoonController::run_CACC_LV()
std::cout << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
<< ", wantsPlatoon = " << wantsPlatoon << std::endl;
-
- if (inPlatoon && wantsPlatoon)
- {
+ if (inPlatoon && wantsPlatoon) {
updatePcConfig();
c2c.setInnerPlatoonDistance(IPD.get());
return;
- }
-
- if (inPlatoon && !wantsPlatoon)
- {
+ }
+
+ if (inPlatoon && !wantsPlatoon) {
c2c.leavePlatoon();
}
c2cAlive = false;
- curState = PlatoonState::ACC;
+ currState = PlatoonState::ACC;
cruiseControllerNotify();
run_ACC();
- return;
}
}
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index dcd99e90ad75e61dbf5ca4d621451f2b1a03dc2e..a38efed00c924116445fb69ebebcae385b436eff 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -12,53 +12,70 @@ PLUGINLIB_EXPORT_CLASS(car::MavLink, nodelet::Nodelet);
namespace car
{
- MavLink::MavLink(ros::NodeHandle &nh, std::string &name) :
- nh_(nh),
- name_(name)
- {}
-
- MavLink::MavLink() {}
-
- MavLink::~MavLink() {}
-
- void MavLink::onInit()
- {
- NODELET_INFO("MavLink::onInit -- START");
- stmData = nh_.advertise<stmDataMsg>("stmData", 1);
- ccData = nh_.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
- laneData = nh_.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
- rcEnabled = nh_.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
- main = boost::thread(
- [this]()
- {
- int counter = 0;
- ros::Rate rate{1};
- while (ros::ok())
- {
- stmDataMsg msg;
- msg.speed = counter++;
- msg.angle = 0;
- stmData.publish(msg);
- rate.sleep();
- }
- });
- NODELET_INFO("MavLink::onInit -- END");
- }
-
- void MavLink::ccDataCallback(const ccDataMsg::ConstPtr& inMsg)
- {
- std::cout << "MavLink recived new cc data (" << inMsg->speed << ").\n";
- }
-
- void MavLink::laneDataCallback(const laneDataMsg::ConstPtr& inMsg)
- {
+MavLink::MavLink(ros::NodeHandle & nh, std::string & name) :
+ nh_(nh), name_(name)
+{}
+
+MavLink::MavLink()
+{}
+
+MavLink::~MavLink()
+{}
+
+void MavLink::onInit()
+{
+ NODELET_INFO("MavLink::onInit -- START");
+
+ stmData = nh_.advertise<stmDataMsg>("stmData", 1);
+ ccData = nh_.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
+ laneData = nh_.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
+ rcEnabled = nh_.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
+
+ veloxConnection = veloxProtocol::Connection::create(net);
+ veloxConnection->open(
+ "/dev/ttySAC0",
+ [this]
+ { onStmDataReceived(); },
+ [this]
+ { NODELET_INFO("Connection to STM closed!\n"); });
+
+ NODELET_INFO("MavLink::onInit -- END");
+}
+
+void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
+{
+ veloxConnection->setSpeed(convertSpeedToStm(inMsg->speed));
+}
+
+void MavLink::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
+{
std::cout << "MavLink recived new lane data ( ).\n";
- }
-
- void MavLink::rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg)
- {
+}
+
+void MavLink::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
+{
std::cout << "MavLink recived new rc status (" << inMsg->enabled << ").\n";
- }
-
-
+}
+
+void MavLink::onStmDataReceived()
+{
+ stmDataMsg msg;
+ msg.speed = convertSpeedFromStm(veloxConnection->getMeasuredSpeed().get());
+ msg.angle = veloxConnection->getMeasuredSteeringAngle().get();
+ stmData.publish(msg);
+}
+
+float MavLink::convertSpeedToStm(float speed) const
+{
+ // TODO: convert speed!
+ return speed;
+}
+
+float MavLink::convertSpeedFromStm(float speed) const
+{
+ // TODO: convert speed!
+ return speed;
+}
+
+
}
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
index 7a5aa89d2abee81c73d471931ecf68bdfdd6d14c..00dde2966437f9167531cda626a2beb50f9f8b2e 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -1,5 +1,5 @@
#include "ultrasonic/USS_SRF02.h"
-#include <wiringPiI2C.h>
+//#include "wiringPiI2C.h"
#include <iostream>
@@ -24,42 +24,41 @@ const int RANGING_MODE_CM = 0x51;
const int DELAY = 70; //70 ms for ranging to finish
-int main() {
-
- USS_SRF02 uss(0x74);
-
- while(1){
- std::cout << uss.getDistance() << std::endl;
- }
- return 0;
-}
+//int main() {
+//
+// USS_SRF02 uss(0x74);
+//
+// while(1){
+// std::cout << uss.getDistance() << std::endl;
+// }
+// return 0;
+//}
USS_SRF02::USS_SRF02(int devId) {
- this->fd = -1; //no file opened yet
- this->devId = devId;
+ this->fd = -1; //no file opened yet
+ this->devId = devId;
}
int USS_SRF02::getDistance() {
- return 0;
- // int distance;
+ int distance;
- // if(fd == -1){
- // USS_SRF02::setup();
- // }
+// if(fd == -1){
+// USS_SRF02::setup();
+// }
+//
+// wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+// usleep(DELAY * 1000);
+// distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
- // wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
- // usleep(DELAY * 1000);
- // distance = wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
-
- // return distance;
+ return distance;
}
void USS_SRF02::setup() {
- // USS_SRF02::fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
-
- //todo error handling,
- //however wiringPiI2CSetupInterface() calls exit() if it fails
+ //fd = wiringPiI2CSetupInterface(DEVICE, this->devId);
+
+ //todo error handling,
+ //however wiringPiI2CSetupInterface() calls exit() if it fails
}
diff --git a/scripts/.gitignore b/scripts/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..8cd0df3a4a3575a30e46aae503e4a2497afcf182
--- /dev/null
+++ b/scripts/.gitignore
@@ -0,0 +1,2 @@
+.vscode
+.idea
\ No newline at end of file
diff --git a/scripts/remote_control.py b/scripts/remote_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..a4bf382473fac374eb66b3ca62f33da28ea3d3ce
--- /dev/null
+++ b/scripts/remote_control.py
@@ -0,0 +1,194 @@
+import pygame
+import sys
+from pexpect import pxssh
+from pygame.locals import *
+
+
+class OdroidConnection(pxssh.pxssh):
+ def __init__(self, hostname, command_program_path, username="root", password=""):
+ pxssh.pxssh.__init__(self)
+ self.hostname = hostname
+ self.username = username
+ self.password = password
+ self.command_program_path = command_program_path
+ self.connected = False
+
+ def __enter__(self):
+ self.connect()
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.disconnect()
+
+ def connect(self):
+ """
+ Connects to a node via ssh. This may take a few seconds (due to the underlying module).
+ :return:
+ """
+ try:
+ self.login(self.hostname, self.username, self.password,
+ original_prompt="root@odroid:~#")
+ except pxssh.ExceptionPxssh:
+ print("ERROR: failed to connect to odroid!")
+ raise
+ self.connected = True
+ print("Connected to: " + str(self.username) + "@" + str(self.hostname))
+ self.start_command_program()
+
+ def disconnect(self):
+ if self.connected:
+ self.sendline("q")
+ self.logout()
+ self.connected = False
+ print("Disconnected from: " + str(self.username) + "@" + str(self.hostname))
+
+ def start_command_program(self):
+ self.sendline(self.command_program_path)
+
+ def set_speed(self, speed):
+ self.sendline("s " + str(speed))
+
+ def set_angle(self, angle):
+ self.sendline("a " + str(angle))
+
+
+def center(src, dest):
+ src.centerx = dest.centerx
+ src.centery = dest.centery
+
+
+class Button:
+ def __init__(self, rect, label, onPressed=None, onReleased=None):
+ self.rect = rect
+ self.label = label
+ self.onPressed = onPressed
+ self.onReleased = onReleased
+ self.pressed = False
+
+ def press(self):
+ if not self.pressed and self.onPressed is not None:
+ self.onPressed()
+ self.pressed = True
+
+ def release(self):
+ if self.pressed and self.onReleased is not None:
+ self.onReleased()
+ self.pressed = False
+
+ def draw(self, surf):
+ # fill
+ if not self.pressed:
+ pygame.draw.rect(surf, (150, 150, 150), self.rect)
+ else:
+ pygame.draw.rect(surf, (255, 100, 100), self.rect)
+ # frame
+ pygame.draw.rect(surf, (10, 10, 10), self.rect, 1)
+ # label
+ font = pygame.font.Font(None, 36)
+ text = font.render(self.label, True, (10, 10, 10))
+ text_rect = text.get_rect()
+ center(text_rect, self.rect)
+ surf.blit(text, text_rect)
+
+
+if __name__ == '__main__':
+ conn = OdroidConnection("10.5.37.195", "/root/repos/Hochautomatisiertes-Fahren/modules/catkin_ws/src/VeloxProtocolLib/cmake-build-debug/TerminalControl")
+ conn.connect()
+
+ FPS = 60
+ pygame.init()
+ fpsClock = pygame.time.Clock()
+
+ BACKGROUND_COLOR = (255, 255, 255)
+ SCREEN_WIDTH, SCREEN_HEIGHT = 640, 480
+ screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0, 32)
+ background = pygame.Surface(screen.get_size())
+ background = background.convert()
+ background.fill(BACKGROUND_COLOR)
+ background.blit(background, (0,0))
+
+ clock = pygame.time.Clock()
+ pygame.key.set_repeat(1, 40)
+
+ GRIDSIZE=10
+ GRID_WIDTH = SCREEN_WIDTH / GRIDSIZE
+ GRID_HEIGHT = SCREEN_HEIGHT / GRIDSIZE
+ UP = (0, -1)
+ DOWN = (0, 1)
+ LEFT = (-1, 0)
+ RIGHT = (1, 0)
+
+ BUTTON_WIDTH = 200
+ BUTTON_HEIGHT = 100
+ PADDING = 10
+ buttonsSurface = pygame.Surface((3 * BUTTON_WIDTH + 2 * PADDING, 2 * BUTTON_HEIGHT + PADDING))
+ buttonsSurface = buttonsSurface.convert()
+ buttonsSurface.fill(BACKGROUND_COLOR)
+
+ SPEED = 1
+ ANGLE = 20
+ ZERO_SPEED = 0
+ ZERO_ANGLE = 0
+
+ forwards = Button(
+ pygame.Rect(1 * (BUTTON_WIDTH + PADDING), 0 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
+ "FORWARDS",
+ onPressed=lambda: conn.set_speed(SPEED),
+ onReleased=lambda: conn.set_speed(ZERO_SPEED))
+
+ backwards = Button(
+ pygame.Rect(1 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
+ "BACKWARDS",
+ onPressed=lambda: conn.set_speed(-SPEED),
+ onReleased=lambda: conn.set_speed(ZERO_SPEED))
+
+ left = Button(
+ pygame.Rect(0 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
+ "LEFT",
+ onPressed=lambda: conn.set_angle(-ANGLE),
+ onReleased=lambda: conn.set_angle(ZERO_ANGLE))
+
+ right = Button(
+ pygame.Rect(2 * (BUTTON_WIDTH + PADDING), 1 * (BUTTON_HEIGHT + PADDING), BUTTON_WIDTH, BUTTON_HEIGHT),
+ "RIGHT",
+ onPressed=lambda: conn.set_angle(ANGLE),
+ onReleased=lambda: conn.set_angle(ZERO_ANGLE))
+
+ buttons = [forwards, backwards, left, right]
+ buttons_rect = buttonsSurface.get_rect()
+ center(buttons_rect, background.get_rect())
+
+ while True:
+ for event in pygame.event.get():
+ if event.type == QUIT:
+ conn.disconnect()
+ pygame.quit()
+ sys.exit()
+ elif event.type == KEYDOWN:
+ if event.key == K_UP:
+ forwards.press()
+ elif event.key == K_DOWN:
+ backwards.press()
+ elif event.key == K_LEFT:
+ left.press()
+ elif event.key == K_RIGHT:
+ right.press()
+ elif event.type == KEYUP:
+ if event.key == K_UP:
+ forwards.release()
+ elif event.key == K_DOWN:
+ backwards.release()
+ elif event.key == K_LEFT:
+ left.release()
+ elif event.key == K_RIGHT:
+ right.release()
+
+ background.fill(BACKGROUND_COLOR)
+
+ for button in buttons:
+ button.draw(buttonsSurface)
+
+ background.blit(buttonsSurface, buttons_rect)
+ screen.blit(background, (0, 0))
+ pygame.display.update()
+ fpsClock.tick(FPS)
\ No newline at end of file