diff --git a/doc/logBuch.md b/doc/logBuch.md
index 8bf120d687524e04daf07e7868ca893efaed65d3..cb5eceb714553d1baff7c7bfa04f9c5ee05a8c2f 100644
--- a/doc/logBuch.md
+++ b/doc/logBuch.md
@@ -109,4 +109,14 @@ Mo. 05.02. (Schlingloff)
Mo. 12.02. (Schlingloff)
----------------------------------------------------------------
--
\ No newline at end of file
+- 14/8 LiPo-Akkus, ODROIDS, Shifter sind geliefert worden
+- Besprechung zukünftiger Termine:
+ - 04.04. (deadline) Kommunikationstest
+ - 16.04. 10.00 Interaktionstest
+ - 16.04. 10.00 Planung der Vorführung
+ - 18.05. 13.00 Generalprobe
+ - 22.05. Endtermin
+
+XX. XX.XX. (Schlingloff)
+----------------------------------------------------------------
+-
\ No newline at end of file
diff --git a/modules/CACC/.gitignore b/doc/modules/cruiseControl/.gitignore
similarity index 100%
rename from modules/CACC/.gitignore
rename to doc/modules/cruiseControl/.gitignore
diff --git a/modules/CACC/CACC-Module-Test.py b/doc/modules/cruiseControl/CACC-Module-Test.py
similarity index 100%
rename from modules/CACC/CACC-Module-Test.py
rename to doc/modules/cruiseControl/CACC-Module-Test.py
diff --git a/modules/CACC/car.py b/doc/modules/cruiseControl/car.py
similarity index 100%
rename from modules/CACC/car.py
rename to doc/modules/cruiseControl/car.py
diff --git a/modules/CACC/moduleCACC.tex b/doc/modules/cruiseControl/docCACC-content.tex
similarity index 58%
rename from modules/CACC/moduleCACC.tex
rename to doc/modules/cruiseControl/docCACC-content.tex
index 559a5f7fd0840236dfecdf100d35e96ed009d23e..02bf4ac52d39011fa05a97e2a205ecc6cf3dde0b 100644
--- a/modules/CACC/moduleCACC.tex
+++ b/doc/modules/cruiseControl/docCACC-content.tex
@@ -1,91 +1,10 @@
-\documentclass[fleqn,a4paper,12pt]{article}
-\usepackage{geometry}
-\geometry{left=30mm, right=40mm, bottom=30mm}
-\usepackage[german]{babel}
-\usepackage[utf8]{inputenc} %Für Umlaute
-
-\usepackage{amsmath} % Mathematische Symbole
-\usepackage{amssymb} % Nochmehr mathematische Symbole
-\usepackage{stmaryrd} % Für die [[ ]] Klammern (\llbracket, \rrbracket)
-\usepackage{dsfont} % Schriftsatz fuer Zahlenmengensymbole
-%\usepackage{verbatim} % erweiterte Verbatim-Umgebung
-\usepackage{alltt} % Quasi-Verbatim-Umgebung
-\usepackage{fancyhdr} % Eigene Kopfzeilen
-\usepackage{graphics} % Zum Einbinden von Grafiken
-\usepackage{tikz} % Zur Erstellung von Graphen
-\usepackage{listings} % Pseudo-Code Formatierung
-% Einbinden einer eps-Grafik geht so: includegraphics{path}
-
-% Seitenraender
-\addtolength{\voffset}{-2cm}
-\addtolength{\textheight}{0cm}
-\addtolength{\hoffset}{0cm}
-\addtolength{\textwidth}{2cm}
-%\addtolength{\headheight}{2cm} % fuer jeden Strichkode einen Zentimeter
-
-% Skalierung der Grafiken
-\setlength{\unitlength}{1cm}
-
-\pagestyle{fancy} % Eigene Kopfzeilen verwenden
-\frenchspacing % Kein Extrafreiraum nach Satzzeichen
-\setlength{\parindent}{0pt} % Neue Absaetze nicht einruecken
-%\sloppy % Schlampige Absatzformatierung
-\fussy % Penible Absatzformatierung
-\linespread{1.5} % Zeilenabstand
-
-%tikz-definition
-\tikzstyle{rec} = [minimum width=1cm, minimum height=0.8cm, align = left,text centered, draw=black]
-
-%andere Definitionen
-\newcommand{\R}{{\mathbb R}}
-\newcommand{\N}{{\mathbb N}}
-\newcommand{\Z}{{\mathbb Z}}
-\newcommand{\Q}{{\mathbb Q}}
-\newcommand{\C}{{\mathbb C}}
-\newcommand{\F}{\mathcal{F}}
-\newcommand{\less}{\setminus}
-\newcommand{\inv}{{}^{-1}}
-\newcommand{\Land}{\bigwedge}
-\newcommand{\Lor}{\bigvee}
-
-% Kopfzeile
-%\lhead{}
-%\rhead{}
-\lstset{ %
- %backgroundcolor=\color{}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor}; should come as last argument
- basicstyle=\footnotesize, % the size of the fonts that are used for the code
- breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace
- breaklines=true, % sets automatic line breaking
- captionpos=b, % sets the caption-position to bottom
- %commentstyle=\color{mygreen}, % comment style
- %deletekeywords={...}, % if you want to delete keywords from the given language
- escapeinside={\%*}{*)}, % if you want to add LaTeX within your code
- extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8
- %frame=single, % adds a frame around the code
- keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible)
- keywordstyle=\color{blue}, % keyword style
- language=Python, % the language of the code
- %morekeywords={*,...}, % if you want to add more keywords to the set
- numbers=left, % where to put the line-numbers; possible values are (none, left, right)
- numbersep=5pt, % how far the line-numbers are from the code
- numberstyle=\tiny,%\color{mygray}, % the style that is used for the line-numbers
- %rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here))
- showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces'
- showstringspaces=false, % underline spaces within strings only
- showtabs=false, % show tabs within strings adding particular underscores
- stepnumber=1, % the step between two line-numbers. If it's 1, each line will be numbered
- stringstyle=\color{mymauve}, % string literal style
- tabsize=2, % sets default tabsize to 2 spaces
- title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title
-}
-\begin{document}
- \section{CACC-Modul}
- \subsection{Problembeschreibung und Einordnung}
+\subsection{CACC-Modul}
+ \subsubsection{Problembeschreibung und Einordnung}
Das \textit{CACC}-Modul (Cooperativ adaptive curse controll) hat die Aufgabe dem eigenen Fahrzeug (eFZ) aus seinen aktuellen Daten und den Platoon-Daten eine neue Geschwindigkeit zu ermitteln, die das eFZ anstrebt um die gegebenen IPD und PS herzustellen und um Ziehharmonika-Effekte und Kollisionen zu vermeiden. Dafür stehen dem eFZ alle fahrzeuginternen sowie die Platoon-Daten IPD und PS zur Verfügung.\\
Die ermittelte Geschwindigkeit wird dann an den einen Controller weiter gegeben, der diese stetig auf den Motor umsetzt.\\
\\
\textit{Versuchsaufbau}\\
- \subsection{Ansätze und Herausforderungen}
+ \subsubsection{Ansätze und Herausforderungen}
Dazu gab es direkt 2 Ansätze. Der erste ist eine 2-State-Machine, die abwechselnd zwischen \textit{IPD herstellen} und \textit{PS herstellen} schaltet.\\
\\
\textit{State-Machine}\\
@@ -124,7 +43,7 @@ while (True):
\end{align*}
Damit lässt sich $v_o^\prime = f(v_o, d_o, v_p, d_{IPD}, v_{PS})$ darstellen.
- \subsection{Lösung}
+ \subsubsection{Lösung}
Zunächst betrachtet man folgende 3 Ungleichungen
\begin{align*}
d_{IPD} - tol(IPD)\le &d_o \le d_{IPD} + tol(IPD)\\
@@ -154,12 +73,6 @@ while (True):
\\
Also lässt $v_o^\prime = f(v_o, d_o, v_p, d_{IPD}, v_{PS}) = g(d_o, v_p, d_{IPD}, v_{PS})$ ausdrücken, da die optimale Geschwindigkeit, nicht von der $v_o$ abhängt oder abhängen muss, da der nach geschaltete Controller eigenständig von der aktuellen Geschwindigkeit $v_o$ auf $v_o^\prime$ reguliert.
- \subsection{Validierung}
+ \subsubsection{Validierung}
- \subsection{Implementierung}
-
-
-\end{document}
-
-
-
+ \subsubsection{Implementierung}
diff --git a/doc/modules/cruiseControl/docCACC-standalone.tex b/doc/modules/cruiseControl/docCACC-standalone.tex
new file mode 100644
index 0000000000000000000000000000000000000000..8467d82b10281bd52c2a9f4215361bfa9dddfe54
--- /dev/null
+++ b/doc/modules/cruiseControl/docCACC-standalone.tex
@@ -0,0 +1,74 @@
+\documentclass[fleqn,a4paper,12pt]{article}
+\usepackage{geometry}
+\geometry{left=30mm, right=40mm, bottom=30mm}
+\usepackage[german]{babel}
+\usepackage[utf8]{inputenc} %Für Umlaute
+
+\usepackage{amsmath} % Mathematische Symbole
+\usepackage{amssymb} % Nochmehr mathematische Symbole
+\usepackage{stmaryrd} % Für die [[ ]] Klammern (\llbracket, \rrbracket)
+\usepackage{dsfont} % Schriftsatz fuer Zahlenmengensymbole
+%\usepackage{verbatim} % erweiterte Verbatim-Umgebung
+\usepackage{alltt} % Quasi-Verbatim-Umgebung
+\usepackage{fancyhdr} % Eigene Kopfzeilen
+\usepackage{graphics} % Zum Einbinden von Grafiken
+\usepackage{tikz} % Zur Erstellung von Graphen
+\usepackage{listings} % Pseudo-Code Formatierung
+% Einbinden einer eps-Grafik geht so: includegraphics{path}
+
+% Seitenraender
+\addtolength{\voffset}{-2cm}
+\addtolength{\textheight}{0cm}
+\addtolength{\hoffset}{0cm}
+\addtolength{\textwidth}{2cm}
+%\addtolength{\headheight}{2cm} % fuer jeden Strichkode einen Zentimeter
+
+% Skalierung der Grafiken
+\setlength{\unitlength}{1cm}
+
+\pagestyle{fancy} % Eigene Kopfzeilen verwenden
+\frenchspacing % Kein Extrafreiraum nach Satzzeichen
+\setlength{\parindent}{0pt} % Neue Absaetze nicht einruecken
+%\sloppy % Schlampige Absatzformatierung
+\fussy % Penible Absatzformatierung
+\linespread{1.5} % Zeilenabstand
+
+%tikz-definition
+\tikzstyle{rec} = [minimum width=1cm, minimum height=0.8cm, align = left,text centered, draw=black]
+
+% Kopfzeile
+%\lhead{}
+%\rhead{}
+\lstset{ %
+ %backgroundcolor=\color{}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor}; should come as last argument
+ basicstyle=\footnotesize, % the size of the fonts that are used for the code
+ breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace
+ breaklines=true, % sets automatic line breaking
+ captionpos=b, % sets the caption-position to bottom
+ %commentstyle=\color{mygreen}, % comment style
+ %deletekeywords={...}, % if you want to delete keywords from the given language
+ escapeinside={\%*}{*)}, % if you want to add LaTeX within your code
+ extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8
+ %frame=single, % adds a frame around the code
+ keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible)
+ keywordstyle=\color{blue}, % keyword style
+ language=Python, % the language of the code
+ %morekeywords={*,...}, % if you want to add more keywords to the set
+ numbers=left, % where to put the line-numbers; possible values are (none, left, right)
+ numbersep=5pt, % how far the line-numbers are from the code
+ numberstyle=\tiny,%\color{mygray}, % the style that is used for the line-numbers
+ %rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here))
+ showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces'
+ showstringspaces=false, % underline spaces within strings only
+ showtabs=false, % show tabs within strings adding particular underscores
+ stepnumber=1, % the step between two line-numbers. If it's 1, each line will be numbered
+ stringstyle=\color{mymauve}, % string literal style
+ tabsize=2, % sets default tabsize to 2 spaces
+ title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title
+}
+\begin{document}
+ \input{docCACC-content.tex}
+\end{document}
+
+
+
diff --git a/modules/CACC/kalmanfilter.py b/doc/modules/cruiseControl/kalmanfilter.py
similarity index 100%
rename from modules/CACC/kalmanfilter.py
rename to doc/modules/cruiseControl/kalmanfilter.py
diff --git a/modules/CACC/simlifiedSpeedVisualization.py b/doc/modules/cruiseControl/simlifiedSpeedVisualization.py
similarity index 100%
rename from modules/CACC/simlifiedSpeedVisualization.py
rename to doc/modules/cruiseControl/simlifiedSpeedVisualization.py
diff --git a/doc/modules/environment/docEnvironment-content.tex b/doc/modules/environment/docEnvironment-content.tex
new file mode 100644
index 0000000000000000000000000000000000000000..eb7ef2e37f53592355425158bda4675e989f1c91
--- /dev/null
+++ b/doc/modules/environment/docEnvironment-content.tex
@@ -0,0 +1,10 @@
+\subsection{Environment-Modul}
+ \subsubsection{Problembeschreibung und Einordnung}
+
+ \subsubsection{Ansätze und Herausforderungen}
+
+ \subsubsection{Lösung}
+
+ \subsubsection{Validierung}
+
+ \subsubsection{Implementierung}
diff --git a/doc/modules/environment/docEnvironment-standalone.tex b/doc/modules/environment/docEnvironment-standalone.tex
new file mode 100644
index 0000000000000000000000000000000000000000..33aa438f7408f0a91c4aec09bca1d61b3442a097
--- /dev/null
+++ b/doc/modules/environment/docEnvironment-standalone.tex
@@ -0,0 +1,74 @@
+\documentclass[fleqn,a4paper,12pt]{article}
+\usepackage{geometry}
+\geometry{left=30mm, right=40mm, bottom=30mm}
+\usepackage[german]{babel}
+\usepackage[utf8]{inputenc} %Für Umlaute
+
+\usepackage{amsmath} % Mathematische Symbole
+\usepackage{amssymb} % Nochmehr mathematische Symbole
+\usepackage{stmaryrd} % Für die [[ ]] Klammern (\llbracket, \rrbracket)
+\usepackage{dsfont} % Schriftsatz fuer Zahlenmengensymbole
+%\usepackage{verbatim} % erweiterte Verbatim-Umgebung
+\usepackage{alltt} % Quasi-Verbatim-Umgebung
+\usepackage{fancyhdr} % Eigene Kopfzeilen
+\usepackage{graphics} % Zum Einbinden von Grafiken
+\usepackage{tikz} % Zur Erstellung von Graphen
+\usepackage{listings} % Pseudo-Code Formatierung
+% Einbinden einer eps-Grafik geht so: includegraphics{path}
+
+% Seitenraender
+\addtolength{\voffset}{-2cm}
+\addtolength{\textheight}{0cm}
+\addtolength{\hoffset}{0cm}
+\addtolength{\textwidth}{2cm}
+%\addtolength{\headheight}{2cm} % fuer jeden Strichkode einen Zentimeter
+
+% Skalierung der Grafiken
+\setlength{\unitlength}{1cm}
+
+\pagestyle{fancy} % Eigene Kopfzeilen verwenden
+\frenchspacing % Kein Extrafreiraum nach Satzzeichen
+\setlength{\parindent}{0pt} % Neue Absaetze nicht einruecken
+%\sloppy % Schlampige Absatzformatierung
+\fussy % Penible Absatzformatierung
+\linespread{1.5} % Zeilenabstand
+
+%tikz-definition
+\tikzstyle{rec} = [minimum width=1cm, minimum height=0.8cm, align = left,text centered, draw=black]
+
+% Kopfzeile
+%\lhead{}
+%\rhead{}
+\lstset{ %
+ %backgroundcolor=\color{}, % choose the background color; you must add \usepackage{color} or \usepackage{xcolor}; should come as last argument
+ basicstyle=\footnotesize, % the size of the fonts that are used for the code
+ breakatwhitespace=false, % sets if automatic breaks should only happen at whitespace
+ breaklines=true, % sets automatic line breaking
+ captionpos=b, % sets the caption-position to bottom
+ %commentstyle=\color{mygreen}, % comment style
+ %deletekeywords={...}, % if you want to delete keywords from the given language
+ escapeinside={\%*}{*)}, % if you want to add LaTeX within your code
+ extendedchars=true, % lets you use non-ASCII characters; for 8-bits encodings only, does not work with UTF-8
+ %frame=single, % adds a frame around the code
+ keepspaces=true, % keeps spaces in text, useful for keeping indentation of code (possibly needs columns=flexible)
+ keywordstyle=\color{blue}, % keyword style
+ language=Python, % the language of the code
+ %morekeywords={*,...}, % if you want to add more keywords to the set
+ numbers=left, % where to put the line-numbers; possible values are (none, left, right)
+ numbersep=5pt, % how far the line-numbers are from the code
+ numberstyle=\tiny,%\color{mygray}, % the style that is used for the line-numbers
+ %rulecolor=\color{black}, % if not set, the frame-color may be changed on line-breaks within not-black text (e.g. comments (green here))
+ showspaces=false, % show spaces everywhere adding particular underscores; it overrides 'showstringspaces'
+ showstringspaces=false, % underline spaces within strings only
+ showtabs=false, % show tabs within strings adding particular underscores
+ stepnumber=1, % the step between two line-numbers. If it's 1, each line will be numbered
+ stringstyle=\color{mymauve}, % string literal style
+ tabsize=2, % sets default tabsize to 2 spaces
+ title=\lstname % show the filename of files included with \lstinputlisting; also try caption instead of title
+}
+\begin{document}
+ \input{docEnvironment-content.tex}
+\end{document}
+
+
+
diff --git a/modules/CACC/moduleCACC.pdf b/modules/CACC/moduleCACC.pdf
deleted file mode 100644
index 9be862f69c9ca3f2770069c1760408d97823343d..0000000000000000000000000000000000000000
Binary files a/modules/CACC/moduleCACC.pdf and /dev/null differ
diff --git a/modules/catkin_ws/src/autonom_a/CMakeLists.txt b/modules/catkin_ws/src/autonom_a/CMakeLists.txt
index 3c4eae4cab3ec65c8120dec62ff7d2afa84e46e7..c31b5ab88f1ba78a156110a646f1397be95e6cd4 100644
--- a/modules/catkin_ws/src/autonom_a/CMakeLists.txt
+++ b/modules/catkin_ws/src/autonom_a/CMakeLists.txt
@@ -32,8 +32,8 @@ catkin_package(
include_directories(
include
- include/testnodelets
include/testnodelets2
+ include/testnodelets
${catkin_INCLUDE_DIRS}
)
@@ -61,8 +61,8 @@ install(
install(
DIRECTORY
- include/testnodelets/
include/testnodelets2/
+ include/testnodelets/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 20b046a7e23e87ff158a385c59f95c36ea06e87c..5cb136286cd0870a172d0bcfedab3fe3022c3184 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -14,6 +14,15 @@ find_package(Boost REQUIRED COMPONENTS system)
add_message_files(
FILES
+ camDataMsg.msg
+ ccDataMsg.msg
+ environmentDataMsg.msg
+ logEnabledMsg.msg
+ logStringMsg.msg
+ rcEnabledMsg.msg
+ stmDataMsg.msg
+ ussDataMsg.msg
+ laneDataMsg.msg
)
generate_messages(
DEPENDENCIES
@@ -22,7 +31,10 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
- #LIBRARIES nodelet_talker
+ LIBRARIES main_node
+ LIBRARIES environment
+ LIBRARIES ultrasonic
+ LIBRARIES camera
CATKIN_DEPENDS roscpp std_msgs nodelet rospy #message_runtime
)
@@ -31,13 +43,32 @@ include_directories(
${catkin_INCLUDE_DIRS}
)
-#add_library(nodelet_talker src/testnodelets/NodeletTalker.cpp)
-#add_dependencies(nodelet_talker ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
-#target_link_libraries(nodelet_talker ${catkin_LIBRARIES})
+add_library(main_node src/mainNode/mainNode.cpp)
+add_dependencies(main_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(main_node ${catkin_LIBRARIES})
+
+add_library(environment src/environment/environment.cpp)
+add_dependencies(environment ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(environment ${catkin_LIBRARIES})
+
+add_library(ultrasonic src/ultrasonic/ultrasonic.cpp)
+add_dependencies(ultrasonic ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(ultrasonic ${catkin_LIBRARIES})
+
+add_library(camera src/camera/camera.cpp)
+add_dependencies(camera ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(camera ${catkin_LIBRARIES})
+
+add_library(lanekeeping src/lanekeeping/lanekeeping.cpp)
+add_dependencies(lanekeeping ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(lanekeeping ${catkin_LIBRARIES})
install(
TARGETS
- #nodelet_talker
+ main_node
+ environment
+ ultrasonic
+ camera
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -45,7 +76,7 @@ install(
install(
DIRECTORY
- #include/testnodelets/
+ include/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
diff --git a/modules/catkin_ws/src/car/include/camera/camera.h b/modules/catkin_ws/src/car/include/camera/camera.h
new file mode 100644
index 0000000000000000000000000000000000000000..6d86a30843e89a50392e8b1aa6ac5154c87f1684
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/camera/camera.h
@@ -0,0 +1,24 @@
+#ifndef CAMERA_H
+#define CAMERA_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "boost/thread.hpp"
+
+namespace car
+{
+ class Camera : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ Camera(ros::NodeHandle &nh, std::string &name);
+ Camera();
+ ~Camera();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+ ros::Publisher camData;
+ boost::thread main;
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/environment/environment.h b/modules/catkin_ws/src/car/include/environment/environment.h
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..58e7ab6871bb5e039e28367aaf389a2e8a222296 100644
--- a/modules/catkin_ws/src/car/include/environment/environment.h
+++ b/modules/catkin_ws/src/car/include/environment/environment.h
@@ -0,0 +1,35 @@
+#ifndef ENVIRONMENT_H
+#define ENVIRONMENT_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+
+#include "car/ussDataMsg.h"
+#include "car/camDataMsg.h"
+
+namespace car
+{
+ class Environment : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ Environment(ros::NodeHandle &nh, std::string &name);
+ Environment();
+ ~Environment();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+
+ float distance;
+ float prevSpeed;
+
+ ros::Publisher environmentData;
+ ros::Subscriber ussData;
+ ros::Subscriber camData;
+
+
+ void ussDataCallback(const ussDataMsg::ConstPtr& inMsg);
+ void camDataCallback(const camDataMsg::ConstPtr& inMsg);
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h b/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h
new file mode 100644
index 0000000000000000000000000000000000000000..9446809bd234e8f7692655aaffceea9d54ee7c20
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h
@@ -0,0 +1,29 @@
+#ifndef LANEKEEPING_H
+#define LANEKEEPING_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+
+#include "car/laneDataMsg.h"
+#include "car/camDataMsg.h"
+
+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);
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/mainNode.h b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c3b73f2cb6c77699da12c3570570372ac4b5578
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
@@ -0,0 +1,28 @@
+#ifndef MAINNODE_H
+#define MAINNODE_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "boost/thread.hpp"
+#include "car/environmentDataMsg.h"
+
+namespace car
+{
+ class MainNode : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ MainNode(ros::NodeHandle &nh, std::string &name);
+ MainNode();
+ ~MainNode();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+ boost::thread loggerModule;
+ ros::Publisher logEnabled;
+ ros::Subscriber environmentData;
+
+ void environmentDataCallback(const environmentDataMsg::ConstPtr& inMsg);
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h b/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h
new file mode 100644
index 0000000000000000000000000000000000000000..d31c30d359849b35c4c1bde236577b82c9390e73
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h
@@ -0,0 +1,24 @@
+#ifndef ULTRASONIC_H
+#define ULTRASONIC_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "boost/thread.hpp"
+
+namespace car
+{
+ class Ultrasonic : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ Ultrasonic(ros::NodeHandle &nh, std::string &name);
+ Ultrasonic();
+ ~Ultrasonic();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+ ros::Publisher ussData;
+ boost::thread main;
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/launch/fullstart.launch b/modules/catkin_ws/src/car/launch/fullstart.launch
new file mode 100644
index 0000000000000000000000000000000000000000..5c39d9d8d3041864d4de71eaaa57595cdc979557
--- /dev/null
+++ b/modules/catkin_ws/src/car/launch/fullstart.launch
@@ -0,0 +1,15 @@
+<launch>
+ <node pkg="nodelet" type="nodelet" name="Master" args="manager" output="screen">
+ <param name="num_worker_threads" value="16" />
+ </node>
+
+ <node pkg="nodelet" type="nodelet" name="nodelet_main_node" args="load car/main_node Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_environment" args="load car/environment Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_ultrasonic" args="load car/ultrasonic Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_camera" args="load car/camera Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_lanekeeping" args="load car/lanekeeping Master"/>
+</launch>
+
+<!--
+ <node pkg="nodelet" type="nodelet" name="" args="load car/ Master"/>
+-->
diff --git a/modules/catkin_ws/src/car/msg/enableLogging.msg b/modules/catkin_ws/src/car/msg/camDataMsg.msg
similarity index 100%
rename from modules/catkin_ws/src/car/msg/enableLogging.msg
rename to modules/catkin_ws/src/car/msg/camDataMsg.msg
diff --git a/modules/catkin_ws/src/car/msg/ccDataMsg.msg b/modules/catkin_ws/src/car/msg/ccDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..81eb829345ac92f4a37e5ab2ab6e5a3e84d8d47e
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/ccDataMsg.msg
@@ -0,0 +1 @@
+float32 speed
diff --git a/modules/catkin_ws/src/car/msg/environmentDataMsg.msg b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..8fb4625f1e311645dd1abfccfd3bf23721df5dc6
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
@@ -0,0 +1,2 @@
+float32 distance
+float32 prevSpeed
diff --git a/modules/catkin_ws/src/car/msg/laneDataMsg.msg b/modules/catkin_ws/src/car/msg/laneDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/modules/catkin_ws/src/car/msg/logEnabledMsg.msg b/modules/catkin_ws/src/car/msg/logEnabledMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..c8151d3f6beab63ac02322ea099f166c034ae46f
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/logEnabledMsg.msg
@@ -0,0 +1 @@
+bool enabled
diff --git a/modules/catkin_ws/src/car/msg/logStringMsg.msg b/modules/catkin_ws/src/car/msg/logStringMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..37f691ff05aa392a7d9ef31ab48773e332784619
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/logStringMsg.msg
@@ -0,0 +1 @@
+string logMsg
diff --git a/modules/catkin_ws/src/car/msg/rcEnabledMsg.msg b/modules/catkin_ws/src/car/msg/rcEnabledMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..c8151d3f6beab63ac02322ea099f166c034ae46f
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/rcEnabledMsg.msg
@@ -0,0 +1 @@
+bool enabled
diff --git a/modules/catkin_ws/src/car/msg/stmDataMsg.msg b/modules/catkin_ws/src/car/msg/stmDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..0f41f8e38986619f2013aa86e0cd292ca667afc4
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/stmDataMsg.msg
@@ -0,0 +1,2 @@
+float32 speed
+float32 angle
diff --git a/modules/catkin_ws/src/car/msg/ussDataMsg.msg b/modules/catkin_ws/src/car/msg/ussDataMsg.msg
new file mode 100644
index 0000000000000000000000000000000000000000..5cd4939c331527d5422b2fedc60172f7fde80262
--- /dev/null
+++ b/modules/catkin_ws/src/car/msg/ussDataMsg.msg
@@ -0,0 +1 @@
+float32 distance
diff --git a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
index bc04715781387c1bfb1358845a43f07cf3f7f714..6726b019505016846a9f355cb3aeb9c384e75928 100644
--- a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
+++ b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
@@ -1,11 +1,55 @@
-<!--
-<library path="lib/libnodelet_talker">
- <class name="autonom_a/nodelet_talker"
- type="autonom_a::NodeletTalker"
+<library path="lib/libmain_node">
+ <class name="car/main_node"
+ type="car::MainNode"
base_class_type="nodelet::Nodelet">
<description>
- This is my.
+ This node holds the threads:
+ - C2C
+ - PC2C
+ - LOG
+ - PLC
+ - EGM
+ - CC
+ </description>
+ </class>
+</library>
+
+<library path="lib/libenvironment">
+ <class name="car/environment"
+ type="car::Environment"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
+ </description>
+ </class>
+</library>
+
+<library path="lib/libultrasonic">
+ <class name="car/ultrasonic"
+ type="car::Ultrasonic"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
+ </description>
+ </class>
+</library>
+
+<library path="lib/libcamera">
+ <class name="car/camera"
+ type="car::Camera"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
+ </description>
+ </class>
+</library>
+
+<library path="lib/liblanekeeping">
+ <class name="car/lanekeeping"
+ type="car::Lanekeeping"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
</description>
</class>
</library>
--->
diff --git a/modules/catkin_ws/src/car/src/camera/camera.cpp b/modules/catkin_ws/src/car/src/camera/camera.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..699b1810afb7e56aa1f9ba191b00ffacefe806ca
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/camera/camera.cpp
@@ -0,0 +1,30 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "camera/camera.h"
+
+#include "car/camDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::Camera, nodelet::Nodelet);
+
+namespace car
+{
+ Camera::Camera(ros::NodeHandle &nh, std::string &name) : nh_(nh), name_(name) {}
+ Camera::Camera() {}
+ Camera::~Camera() {}
+ void Camera::onInit()
+ {
+ NODELET_INFO("Camera::onInit -- START");
+ camData = nh_.advertise<camDataMsg>("camData", 1);
+ main = boost::thread([this] () {
+ ros::Rate rate{1};
+ while(ros::ok()) {
+ camDataMsg msg;
+ camData.publish(msg);
+ rate.sleep();
+ }
+ });
+
+ NODELET_INFO("Camera::onInit -- END");
+ }
+}
diff --git a/modules/catkin_ws/src/car/src/environment/environment.cpp b/modules/catkin_ws/src/car/src/environment/environment.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..b1b7e684748bb4fd951ed8c4f79c5ac419c6f03a 100644
--- a/modules/catkin_ws/src/car/src/environment/environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/environment.cpp
@@ -0,0 +1,52 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "environment/environment.h"
+
+#include "car/camDataMsg.h"
+#include "car/environmentDataMsg.h"
+#include "car/ussDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
+
+namespace car
+{
+ Environment::Environment(ros::NodeHandle &nh, std::string &name) :
+ nh_(nh),
+ name_(name),
+ distance(0),
+ prevSpeed(0) {}
+
+ Environment::Environment() {}
+
+ Environment::~Environment() {}
+
+ void Environment::onInit()
+ {
+ NODELET_INFO("Environment::onInit -- START");
+ environmentData = nh_.advertise<environmentDataMsg>("environmentData", 1);
+ ussData = nh_.subscribe("ussData", 1, &Environment::ussDataCallback, this);
+ camData = nh_.subscribe("camData", 1, &Environment::camDataCallback, this);
+ NODELET_INFO("Environment::onInit -- END");
+ }
+
+ void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
+ {
+ std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
+ distance = inMsg->distance;
+ prevSpeed = 1.0;
+
+ environmentDataMsg outMsg;
+ outMsg.distance = distance;
+ outMsg.prevSpeed = prevSpeed;
+
+ environmentData.publish(outMsg);
+ }
+
+ void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
+ {
+ std::cout << "Environment recived new cam data ( ).\n";
+ }
+
+
+}
diff --git a/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..363f3d5456c2acc442589988aa52d760cd480c5b
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp
@@ -0,0 +1,36 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "lanekeeping/lanekeeping.h"
+
+#include "car/camDataMsg.h"
+#include "car/environmentDataMsg.h"
+
+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)
+ {
+ std::cout << "Lanekeeping recived new cam data ( ).\n";
+ }
+
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ad5a8b1a17b6177239065dfac39341232980cdf2
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
@@ -0,0 +1,38 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "mainNode/mainNode.h"
+
+#include "car/camDataMsg.h"
+#include "car/ccDataMsg.h"
+#include "car/environmentDataMsg.h"
+#include "car/logEnabledMsg.h"
+#include "car/logStringMsg.h"
+#include "car/rcEnabledMsg.h"
+#include "car/stmDataMsg.h"
+#include "car/ussDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::MainNode, nodelet::Nodelet);
+
+namespace car
+{
+ MainNode::MainNode(ros::NodeHandle &nh, std::string &name) : nh_(nh), name_(name) {}
+ MainNode::MainNode() {}
+ MainNode::~MainNode() {}
+ void MainNode::onInit()
+ {
+ NODELET_INFO("MainNode::onInit -- START");
+ logEnabled = nh_.advertise<logEnabledMsg>("logEnabled", 5);
+ environmentData = nh_.subscribe("environmentData", 1, &MainNode::environmentDataCallback, this);
+ std::cout << "MainNode &nh_: " << &nh_ << "\n";
+
+
+ // CODE GOES HERE
+ NODELET_INFO("MainNode::onInit -- END");
+ }
+
+ void MainNode::environmentDataCallback(const environmentDataMsg::ConstPtr& inMsg)
+ {
+ std::cout << "MainNode recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+ }
+}
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..433b4c52fe8188d046520862cb549319645258ec
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp
@@ -0,0 +1,32 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "ultrasonic/ultrasonic.h"
+
+#include "car/ussDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::Ultrasonic, nodelet::Nodelet);
+
+namespace car
+{
+ Ultrasonic::Ultrasonic(ros::NodeHandle &nh, std::string &name) : nh_(nh), name_(name) {}
+ Ultrasonic::Ultrasonic() {}
+ Ultrasonic::~Ultrasonic() {}
+ void Ultrasonic::onInit()
+ {
+ NODELET_INFO("Ultrasonic::onInit -- START");
+ ussData = nh_.advertise<ussDataMsg>("ussData", 1);
+ main = boost::thread([this] () {
+ int counter = 0;
+ ros::Rate rate{1};
+ while(ros::ok()) {
+ ussDataMsg msg;
+ msg.distance = counter++;
+ ussData.publish(msg);
+ rate.sleep();
+ }
+ });
+
+ NODELET_INFO("Ultrasonic::onInit -- END");
+ }
+}