diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 93bbf7d685aecb9ea26238cd4d9065ba36828e81..26947910a159c1e9663baba339c243ad0efb48b5 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -52,11 +52,15 @@ include_directories(
 
 set(MAIN_NODE_SOURCE_FILES
         include/mainNode/MainNode.h
+        include/mainNode/EgoMotion.h
         include/mainNode/NotifiableThread.h
         include/mainNode/PlatoonController.h
+        include/mainNode/CruiseController.h
         src/mainNode/MainNode.cpp
+        src/mainNode/EgoMotion.cpp
         src/mainNode/NotifiableThread.cpp
-        src/mainNode/PlatoonController.cpp)
+        src/mainNode/PlatoonController.cpp
+        src/mainNode/CruiseController.cpp)
 
 set(LOCAL_INSTALL_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install)
 
diff --git a/modules/catkin_ws/src/car/include/mainNode/CruiseController.h b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd8977e215e26a22c4c449e07f58ecdff1964e08
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/mainNode/CruiseController.h
@@ -0,0 +1,22 @@
+#ifndef CRUISECONTROLLER_H
+#define CRUISECONTROLLER_H
+
+#include "PlatoonController.h"
+#include "EgoMotion.h"
+
+namespace car
+{
+class CruiseController 
+{
+  public:
+    CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController);
+    ~CruiseController(){};
+
+    void run();
+    
+  private:
+    EgoMotion& egoMotion;
+    PlatoonController& platoonController;
+};
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index aac0199006346b6366b0a5558c859e7e1c80cfcd..d398bd90b53bfcad75df7b82ec7396bfa0b6b7e6 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -1,14 +1,31 @@
 #ifndef EGOMOTION_H
 #define EGOMOTION_H
 
-#endif
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+
+#include <functional>
+
+#include "car/environmentDataMsg.h"
 
 namespace car
 {
 class EgoMotion 
 {
   public:
-    EgoMotion(){};
+    using Callback = std::function<void()>;
+    
+    EgoMotion(ros::NodeHandle nh);
     ~EgoMotion(){};
+
+    Callback platoonControllerNotify;
+    Callback cruiseControllerNotify;
+    
+  private:
+    ros::Subscriber environmentData;
+    void environmentDataCallback(const environmentDataMsg::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
index 046ba523f0cb48e89ca8244d78cc3882ed462410..60a4c812b5b27fc6d0745b8fab39b9e4141020ee 100644
--- a/modules/catkin_ws/src/car/include/mainNode/MainNode.h
+++ b/modules/catkin_ws/src/car/include/mainNode/MainNode.h
@@ -4,7 +4,6 @@
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
 #include "boost/thread.hpp"
-#include "car/environmentDataMsg.h"
 
 #include "NotifiableThread.h"
 #include "NetworkingLib/Networking.h"
@@ -13,7 +12,7 @@
 
 #include "PlatoonController.h"
 #include "EgoMotion.h"
-// #include "CruiseController.h"
+#include "CruiseController.h"
 
 namespace car
 {
@@ -29,29 +28,29 @@ public:
     ~MainNode();
 
 private:
-    networking::Networking c2cNet;
-    networking::Networking pcNet;
+    ros::NodeHandle nh;
+    std::string name;
+    
+    networking::Networking c2cNet; // thread  + event queue
+    networking::Networking pcNet; // thread  + event queue
+    
+    platoonProtocol::Vehicle::Ptr c2c;
+    pc2car::CommandReceiver::Ptr pc;
 
     EgoMotion egoMotion;
     
     PlatoonController platoonController;
     NotifiableThread platoonControllerThread;
      
-     std::function<void()> platoonControllerNotify =
-         [this] { platoonControllerThread.notify(); };
-     
-    // CruiseController cruiseController;
-
-    platoonProtocol::Vehicle::Ptr c2c;
-    pc2car::CommandReceiver::Ptr commandReceiver;
+    CruiseController cruiseController;
+    NotifiableThread cruiseControllerThread;
       
-    ros::NodeHandle nh;
-    std::string name;
+    std::function<void()> platoonControllerNotify =
+        [this] { platoonControllerThread.notify(); };
+    
+    std::function<void()> cruiseControllerNotify =
+        [this] { cruiseControllerThread.notify(); };
     
-    ros::Publisher logEnabled;
-
-    ros::Subscriber environmentData;
-    void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
 };
 }
 #endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index f14238758d086b12f93f5febeb3f0285e83dda73..982f099bcb8a42cc57c08fb1fe50332869aaf2f4 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -7,6 +7,10 @@
 
 #include <functional>
 
+#include "PlatoonProtocolLib/Vehicle.h"
+#include "PC2CarLib/CommandReceiver.h"
+#include "EgoMotion.h"
+
 namespace car
 {
 
@@ -15,12 +19,19 @@ class PlatoonController
 public:
     using Callback = std::function<void()>;
     
-    PlatoonController();
+    PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+                      pc2car::CommandReceiver::Ptr pc,
+                      EgoMotion& egoMotion);
       
     void run();
+    Callback cruiseControllerNotify;
 
 private:
-    Callback ccNotify;
+
+    platoonProtocol::Vehicle::Ptr c2c;
+    pc2car::CommandReceiver::Ptr pc;
+    EgoMotion& egoMotion;
+
 };
 
 }
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index ed418339494868d9963afe2d01a5116ec54b23c0..df596bbc27333d315c357fcc3da38b717fa91562 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -32,7 +32,7 @@ namespace car
 
   void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
+    // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
     distance = inMsg->distance;
     prevSpeed = 1.0;
     
@@ -45,7 +45,7 @@ namespace car
 
   void Environment::camDataCallback(const camDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Environment recived new cam data ( ).\n";
+    // 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
index b072e46ec8589c13071dc095b922f7f65e3707f4..6ef021265d0ae9b2cc6b7593d348cfcbc64fc5fa 100644
--- a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
+++ b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
@@ -29,7 +29,7 @@ namespace car
 
   void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr& inMsg)
   {
-    std::cout << "Lanekeeping recived new cam data ( ).\n";
+    // std::cout << "Lanekeeping recived new cam data ( ).\n";
   }
   
   
diff --git a/modules/catkin_ws/src/car/src/logging/Logging.cpp b/modules/catkin_ws/src/car/src/logging/Logging.cpp
index 688eacfdee0d3c23348927686752dca0ffac2429..9bfdcf94ac87002c9bdb31fe7e3ca8871b688ac4 100644
--- a/modules/catkin_ws/src/car/src/logging/Logging.cpp
+++ b/modules/catkin_ws/src/car/src/logging/Logging.cpp
@@ -42,47 +42,47 @@ void Logging::onInit()
 
 void Logging::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new cc data ( ).\n";
+    // std::cout << "Logging received new cc data ( ).\n";
 }
 
 void Logging::camDataCallback(const camDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new cam data ( ).\n";
+    // std::cout << "Logging received new cam data ( ).\n";
 }
 
 void Logging::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new environment data ( ).\n";
+    // std::cout << "Logging received new environment data ( ).\n";
 }
 
 void Logging::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new lane data ( ).\n";
+    // std::cout << "Logging received new lane data ( ).\n";
 }
 
 void Logging::logEnabledCallback(const logEnabledMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new log enabled ( ).\n";
+    // std::cout << "Logging received new log enabled ( ).\n";
 }
 
 void Logging::logStringCallback(const logStringMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new log string ( ).\n";
+    // std::cout << "Logging received new log string ( ).\n";
 }
 
 void Logging::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new rc enabled ( ).\n";
+    // std::cout << "Logging received new rc enabled ( ).\n";
 }
 
 void Logging::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new stm data ( ).\n";
+    // std::cout << "Logging received new stm data ( ).\n";
 }
 
 void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "Logging received new uss data ( ).\n";
+    // std::cout << "Logging received new uss data ( ).\n";
 }
 
-}
\ No newline at end of file
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..231b7973c5de412ae5b538aafa59394f363b34c6
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -0,0 +1,18 @@
+#include "../../include/mainNode/CruiseController.h"
+
+#include <iostream>
+
+namespace car {
+
+CruiseController::CruiseController(EgoMotion& egoMotion, PlatoonController& platoonController)
+    : egoMotion(egoMotion)
+      , platoonController(platoonController)
+{
+  std::cout << "WEEEEEEEEEEEEEEEELT" << std::endl;
+}
+
+void CruiseController::run() {
+    std::cout << "CruiseController was run." << std::endl;
+}
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 5a2c6841b9ce23f6e9a213a24fcf80144a1b8cf8..8ad351c01ab99c10c55a71646027a406b5bf3b9c 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -1 +1,19 @@
 #include "mainNode/EgoMotion.h"
+
+
+namespace car {
+  
+EgoMotion::EgoMotion(ros::NodeHandle nh)
+    : platoonControllerNotify()
+      , cruiseControllerNotify()
+{
+    environmentData = nh.subscribe("environmentData", 1, &EgoMotion::environmentDataCallback, this);
+}
+
+void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
+{
+    std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+    cruiseControllerNotify();
+}
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 6e37c055c0e36a8e4ebe8fc587c0738cc938ec53..1cad6c10170a097e7fe9c03228a5dffce0871901 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -1,47 +1,47 @@
-#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"
+#include <pluginlib/class_list_macros.h>
 
 PLUGINLIB_EXPORT_CLASS(car::MainNode, nodelet::Nodelet);
 
 namespace car
 {
 MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
-    c2cNet()
+    nh(nh)  
+    , name(name)
+    , c2cNet()
     , pcNet()
-    , egoMotion()
-    , platoonController()
-    , platoonControllerThread([this] { platoonController.run(); })
-    // , cruiseController()
     , c2c()
-    , commandReceiver(pc2car::CommandReceiver::create(pcNet))
-    , nh(nh)  
-    , name(name)
+    , pc(pc2car::CommandReceiver::create(pcNet))
+    , egoMotion(nh)
+    , platoonController(c2c, pc, egoMotion)
+    , platoonControllerThread([this] { platoonController.run(); })
+    , cruiseController(egoMotion, platoonController)
+    , cruiseControllerThread([this] { cruiseController.run(); })
 {
-  std::cout << "Called MainNode(ros::NodeHandle & nh, std::string & name)." << std::endl;
+  egoMotion.cruiseControllerNotify = cruiseControllerNotify;
+  egoMotion.platoonControllerNotify = platoonControllerNotify;
+  platoonController.cruiseControllerNotify = cruiseControllerNotify;
+  // std::cout << "Called MainNode(ros::NodeHandle & nh, std::string & name)." << std::endl;
 }
 
 MainNode::MainNode() : 
-    c2cNet()
+    nh()  
+    , name()
+    , c2cNet()
     , pcNet()
-    , egoMotion()
-    , platoonController()
-    , platoonControllerThread([this] { platoonController.run(); })
-    // , cruiseController()
     , c2c()
-    , commandReceiver(pc2car::CommandReceiver::create(pcNet))
+    , pc(pc2car::CommandReceiver::create(pcNet))
+    , egoMotion(nh)
+    , platoonController(c2c, pc, egoMotion)
+    , platoonControllerThread([this] { platoonController.run(); })
+    , cruiseController(egoMotion, platoonController)
+    , cruiseControllerThread([this] { cruiseController.run(); })
 {
-  std::cout << "Called MainNode()." << std::endl;
+  egoMotion.cruiseControllerNotify = cruiseControllerNotify;
+  egoMotion.platoonControllerNotify = platoonControllerNotify;
+  platoonController.cruiseControllerNotify = cruiseControllerNotify;
+  // std::cout << "Called MainNode()." << std::endl;
 }
 
 MainNode::~MainNode()
@@ -50,16 +50,7 @@ MainNode::~MainNode()
 void MainNode::onInit()
 {
     NODELET_INFO("MainNode::onInit -- START");
-    logEnabled = nh.advertise<logEnabledMsg>("logEnabled", 5);
-    environmentData = nh.subscribe("environmentData", 1, &MainNode::environmentDataCallback, this);
-
-    // 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/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index fd1aff2bf90581994202b196d226b904e09331a0..3231cc5bcd7f53d331dbd7ef1d513100ce1de90a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -8,9 +8,12 @@
 namespace car
 {
 
-PlatoonController::PlatoonController()
-    : ccNotify()
-{}
+PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+    pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
+    : c2c(c2c)
+      , pc(pc)
+      , egoMotion(egoMotion)
+      , cruiseControllerNotify() {}
 
 
 void car::PlatoonController::run()