diff --git a/modules/catkin_ws/src/PC/src/Logger.cpp b/modules/catkin_ws/src/PC/src/Logger.cpp
index fd7930c6646ce672bcd1080c77e36e0d09d5daef..7d5588cbee61132752774433dd1769a04ec3bcc9 100644
--- a/modules/catkin_ws/src/PC/src/Logger.cpp
+++ b/modules/catkin_ws/src/PC/src/Logger.cpp
@@ -39,7 +39,7 @@ void Logger::log(AsyncState::Ptr state)
             if (!responseMessage.empty())
                 std::cout << responseMessage;
 
-            state->self->timer->startTimeout(100ms, [state]
+            state->self->timer->startTimeout(1ms, [state]
             { state->self->log(state); });
         });
 }
diff --git a/modules/catkin_ws/src/car/include/camera/Camera.h b/modules/catkin_ws/src/car/include/camera/Camera.h
index 6d86a30843e89a50392e8b1aa6ac5154c87f1684..e04b4038bcb1cc76a9362451ee6267815206bc30 100644
--- a/modules/catkin_ws/src/car/include/camera/Camera.h
+++ b/modules/catkin_ws/src/car/include/camera/Camera.h
@@ -3,22 +3,23 @@
 
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
-#include "boost/thread.hpp"
+#include <thread>
+#include <logging/MessageOStream.h>
 
 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;
-	};
+class Camera : public nodelet::Nodelet
+{
+public:
+    Camera();
+
+    void onInit() override;
+
+private:
+    ros::NodeHandle nh;
+    MessageOStream messageOStream;
+    ros::Publisher camData;
+    std::thread cameraThread;
+};
 }
 #endif
diff --git a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
index c94ccd825eada38afff9c9a3bce7d8fc69f7a4e7..d008ec47a30f517937a937850900da44ad7cb0ca 100644
--- a/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
+++ b/modules/catkin_ws/src/car/include/lanekeeping/Lanekeeping.h
@@ -3,6 +3,7 @@
 
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
+#include <logging/MessageOStream.h>
 
 #include "car/laneDataMsg.h"
 #include "car/camDataMsg.h"
@@ -12,18 +13,13 @@ namespace car
 class Lanekeeping : public nodelet::Nodelet
 {
 public:
-    virtual void onInit();
-
-    Lanekeeping(ros::NodeHandle & nh, std::string & name);
-
     Lanekeeping();
 
-    ~Lanekeeping();
+    void onInit() override;
 
 private:
-    ros::NodeHandle nh_;
-    std::string name_;
-
+    ros::NodeHandle nh;
+    MessageOStream messageOStream;
     ros::Publisher laneData;
     ros::Subscriber camData;
 
diff --git a/modules/catkin_ws/src/car/include/logging/Logging.h b/modules/catkin_ws/src/car/include/logging/Logging.h
index 8c9ea5eeb7c7045e81b248832495c651c4e55878..1d2f1641cb2eb4b892c91ebb248c2f0e02330a84 100644
--- a/modules/catkin_ws/src/car/include/logging/Logging.h
+++ b/modules/catkin_ws/src/car/include/logging/Logging.h
@@ -29,7 +29,7 @@ class Logging : public nodelet::Nodelet
 public:
     static constexpr std::size_t PORT{10207};
     static constexpr std::size_t MAX_MESSAGE_SIZE{1024};
-    static constexpr std::size_t MAX_BUFFER_SIZE{0x100000}; // 1MB
+    static constexpr std::size_t MAX_BUFFER_SIZE{std::numeric_limits<std::size_t>::max()};
 
     Logging();
 
@@ -42,26 +42,6 @@ private:
         using ResponseMessage = std::string;
     };
 
-    class OSyncStream
-    {
-    public:
-        OSyncStream(boost::asio::streambuf & buffer, std::mutex & mutex)
-            : stream(&buffer), mutex(mutex)
-        {}
-
-        template<typename T>
-        friend OSyncStream & operator<<(OSyncStream & oSyncStream, const T & value)
-        {
-            std::lock_guard<std::mutex> lock{oSyncStream.mutex};
-            oSyncStream.stream << value;
-            return oSyncStream;
-        }
-
-    private:
-        std::ostream stream;
-        std::mutex & mutex;
-    };
-
     ros::NodeHandle nh_;
     std::string name_;
 
@@ -77,7 +57,7 @@ private:
 
     std::mutex loggingMutex;
     boost::asio::streambuf loggingBuffer;
-    OSyncStream loggingOStream;
+    std::ostream loggingOStream;
 
     networking::Networking net;
     networking::service::Server<LoggingService>::Ptr loggingServer;
diff --git a/modules/catkin_ws/src/car/include/logging/MessageOStream.h b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
index efc179f519081861f6ac274669465a2f830ec4e3..0e568c4da4ff21d13854aaef63a5f6b433c82385 100644
--- a/modules/catkin_ws/src/car/include/logging/MessageOStream.h
+++ b/modules/catkin_ws/src/car/include/logging/MessageOStream.h
@@ -8,6 +8,7 @@
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
 #include <string>
+#include <utility>
 #include "car/logStringMsg.h"
 
 namespace car
@@ -18,32 +19,63 @@ class MessageOStream
 public:
     using Manipulator = MessageOStream & (*)(MessageOStream &);
 
-    explicit MessageOStream(ros::NodeHandle & nh)
-        : logStringPublisher(nh.advertise<logStringMsg>("logString", 1))
+    explicit MessageOStream(ros::NodeHandle & nh, std::string module)
+        : module(std::move(module))
+          , logStringPublisher(nh.advertise<logStringMsg>("logString", 1))
+          , currStream{&ossValue}
     {}
 
     template<typename T>
-    friend MessageOStream & operator<<(MessageOStream & stream, const T & value)
+    friend MessageOStream & operator<<(MessageOStream & stream, const T & data)
     {
-        stream.oss << value;
+        (*stream.currStream) << data;
         return stream;
     }
 
     friend MessageOStream & operator<<(MessageOStream & stream, Manipulator manipulator)
-    { return manipulator(stream); }
+    {
+        return manipulator(stream);
+    }
+
+    template<typename Key, typename Value>
+    MessageOStream & write(const Key & key, const Value & value)
+    {
+        return *this << MessageOStream::key << key
+                     << MessageOStream::value << value
+                     << MessageOStream::flush;
+    }
 
     static MessageOStream & flush(MessageOStream & stream)
     {
         logStringMsg msg;
-        msg.logMsg = stream.oss.str();
+        std::ostringstream result;
+        result << ">> " << stream.module << " [" << stream.ossKey.str() << "] " << stream.ossValue.str() << "\n";
+        msg.logMsg = result.str();
         stream.logStringPublisher.publish(msg);
-        stream.oss = std::ostringstream{};
+        stream.ossValue = std::ostringstream{};
+        stream.ossKey = std::ostringstream{};
+        stream.currStream = &stream.ossValue;
+        return stream;
+    }
+
+    static MessageOStream & value(MessageOStream & stream)
+    {
+        stream.currStream = &stream.ossValue;
+        return stream;
+    }
+
+    static MessageOStream & key(MessageOStream & stream)
+    {
+        stream.currStream = &stream.ossKey;
         return stream;
     }
 
 private:
+    std::string module;
     ros::Publisher logStringPublisher;
-    std::ostringstream oss;
+    std::ostringstream ossValue;
+    std::ostringstream ossKey;
+    std::ostringstream * currStream;
 };
 
 }
diff --git a/modules/catkin_ws/src/car/launch/fullstart.launch b/modules/catkin_ws/src/car/launch/fullstart.launch
index 17c5e8a90af1de7e9026cf401a0420163d2694ab..cf9e223c21a617c0108e35446ceb36714ac604d5 100644
--- a/modules/catkin_ws/src/car/launch/fullstart.launch
+++ b/modules/catkin_ws/src/car/launch/fullstart.launch
@@ -3,13 +3,13 @@ dsdf  <node pkg="nodelet" type="nodelet" name="Master" args="manager" output="sc
     <param name="num_worker_threads" value="16" />
   </node>
 
+  <node pkg="nodelet" type="nodelet" name="nodelet_logging" args="load car/logging Master"/>
   <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"/>
   <node pkg="nodelet" type="nodelet" name="nodelet_mav_link" args="load car/mav_link Master"/>
-  <node pkg="nodelet" type="nodelet" name="nodelet_logging" args="load car/logging Master"/>
 </launch>
 
 <!--
diff --git a/modules/catkin_ws/src/car/src/camera/Camera.cpp b/modules/catkin_ws/src/car/src/camera/Camera.cpp
index fb71a63fe793b516ab2cb6a10a44748308583c49..598d4e96882a9e4daae42ea9836c9c449a990b2d 100644
--- a/modules/catkin_ws/src/car/src/camera/Camera.cpp
+++ b/modules/catkin_ws/src/car/src/camera/Camera.cpp
@@ -10,20 +10,16 @@ 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()
+    : messageOStream(nh, "Camera")
 {}
 
 void Camera::onInit()
 {
-    NODELET_INFO("Camera::onInit -- START");
-    camData = nh_.advertise<camDataMsg>("camData", 1);
-    main = boost::thread(
+    messageOStream.write("onInit", "START");
+
+    camData = nh.advertise<camDataMsg>("camData", 1);
+    cameraThread = std::thread(
         [this]()
         {
             ros::Rate rate{1};
@@ -35,6 +31,6 @@ void Camera::onInit()
             }
         });
 
-    NODELET_INFO("Camera::onInit -- END");
+    messageOStream.write("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 303c6f9a2a5f5393522fd3717bcf06a77628b768..f44b51b5dbbd2a3d98283df74f961bfd95233391 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -12,20 +12,24 @@ namespace car
 {
 
 Environment::Environment()
-    : messageOStream(nh)
+    : messageOStream(nh, "Environment")
       , distance(0)
       , relativeSpeed(0)
 {}
 
 void Environment::onInit()
 {
-    messageOStream << "Environment::onInit -- START" << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "onInit()"
+                   << MessageOStream::value << "START"
+                   << MessageOStream::flush;
 
     environmentData = nh.advertise<environmentDataMsg>("environmentData", 1);
     ussData = nh.subscribe("ussData", 1, &Environment::ussDataCallback, this);
     camData = nh.subscribe("camData", 1, &Environment::camDataCallback, this);
 
-    messageOStream << "Environment::onInit -- END" << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "onInit()"
+                   << MessageOStream::value << "END"
+                   << MessageOStream::flush;
 }
 
 void Environment::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
diff --git a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
index 499be7173cf9149d18137b401e026406d7b77c7a..f059a82d0c146ff7c228df97a1748f8cc5a8cc3e 100644
--- a/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
+++ b/modules/catkin_ws/src/car/src/lanekeeping/Lanekeeping.cpp
@@ -10,22 +10,19 @@ 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()
+    : messageOStream(nh, "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");
+    messageOStream.write("onInit", "START");
+
+    laneData = nh.advertise<laneDataMsg>("laneData", 1);
+    camData = nh.subscribe("camData", 1, &Lanekeeping::camDataCallback, this);
+
+    messageOStream.write("onInit", "END");
 }
 
 void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr & inMsg)
@@ -33,5 +30,4 @@ void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr & inMsg)
     // 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 1e634f170fe360918a9f058d187292fefdec0233..968aefd6db86837d1390d8eea105be118b065907 100644
--- a/modules/catkin_ws/src/car/src/logging/Logging.cpp
+++ b/modules/catkin_ws/src/car/src/logging/Logging.cpp
@@ -17,7 +17,7 @@ constexpr std::size_t Logging::MAX_BUFFER_SIZE;
 
 Logging::Logging()
     : loggingBuffer(MAX_BUFFER_SIZE)
-      , loggingOStream(loggingBuffer, loggingMutex)
+      , loggingOStream(&loggingBuffer)
 {}
 
 void Logging::onInit()
@@ -29,7 +29,7 @@ void Logging::onInit()
     environmentData = nh_.subscribe("environmentData", 1, &Logging::environmentDataCallback, this);
     laneData = nh_.subscribe("laneData", 1, &Logging::laneDataCallback, this);
     logEnabled = nh_.subscribe("logEnabled", 1, &Logging::logEnabledCallback, this);
-    logString = nh_.subscribe("logString", 1, &Logging::logStringCallback, this);
+    logString = nh_.subscribe("logString", 1000, &Logging::logStringCallback, this);
     rcEnabled = nh_.subscribe("rcEnabled", 1, &Logging::rcEnabledCallback, this);
     stmData = nh_.subscribe("stmData", 1, &Logging::stmDataCallback, this);
     ussData = nh_.subscribe("ussData", 1, &Logging::ussDataCallback, this);
@@ -58,7 +58,8 @@ void Logging::camDataCallback(const camDataMsg::ConstPtr & inMsg)
 
 void Logging::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
 {
-    loggingOStream << "environment [distance]: " << "\n";
+    std::lock_guard<std::mutex> lock{loggingMutex};
+    loggingOStream << ">> environmentData [distance] " << inMsg->distance << "\n";
 }
 
 void Logging::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
@@ -73,7 +74,8 @@ void Logging::logEnabledCallback(const logEnabledMsg::ConstPtr & inMsg)
 
 void Logging::logStringCallback(const logStringMsg::ConstPtr & inMsg)
 {
-    loggingOStream << "message: " << inMsg->logMsg << "\n";
+    std::lock_guard<std::mutex> lock{loggingMutex};
+    loggingOStream << inMsg->logMsg;
 }
 
 void Logging::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
@@ -88,7 +90,8 @@ void Logging::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
 
 void Logging::ussDataCallback(const ussDataMsg::ConstPtr & inMsg)
 {
-    loggingOStream << "uss [distance]: " << inMsg->distance << "\n";
+    std::lock_guard<std::mutex> lock{loggingMutex};
+    loggingOStream << ">> ussData [distance] " << inMsg->distance << "\n";
 }
 
 }
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 689073a18f76a88224223596f2e69871dea9c09e..6d4ac4ac2e66e03c0b1c9b99f666ad7f781fbee4 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -9,7 +9,7 @@ CruiseController::CruiseController(ros::NodeHandle & nh,
                                    platoonProtocol::VehicleFacade & c2c,
                                    EgoMotion & egoMotion,
                                    PlatoonController & platoonController)
-    : messageOStream(nh)
+    : messageOStream(nh, "CruiseController")
       , c2c(c2c)
       , egoMotion(egoMotion)
       , platoonController(platoonController)
@@ -43,19 +43,21 @@ void CruiseController::run()
 
 void CruiseController::run_ACC(float target_speed)
 {
-    messageOStream << "CruiseController::run_ACC(" << target_speed << ")" << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "run_ACC"
+                   << MessageOStream::value << "target_speed: " << target_speed;
+
     float v_self = egoMotion.getOwnSpeed();
     float d = egoMotion.getDistance();
     float crash_dist = crash_time * v_self;
 
     if (d < crash_dist)
     {
-        messageOStream << "in crash_dist" << MessageOStream::flush;
+        messageOStream << MessageOStream::value << "\nin crash_dist";
         speed = target_speed * (d - stop_dist) / crash_dist;
     }
     else
     {
-        messageOStream << "not in crash_dist" << MessageOStream::flush;
+        messageOStream << MessageOStream::value << "\nnot in crash_dist";
         speed = target_speed;
     }
 
@@ -64,11 +66,14 @@ void CruiseController::run_ACC(float target_speed)
     ccDataMsg outMsg;
     outMsg.speed = speed;
     ccData.publish(outMsg);
+
+    messageOStream << MessageOStream::flush;
 }
 
 void CruiseController::run_CACC()
 {
-    messageOStream << "CruiseController::run_CACC" << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "run_CACC"
+                   << MessageOStream::flush;
 
     auto config = platoonController.getPlatoonConfig().get();
     float PS = config.platoonSpeed;
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 301a4646dbb94a42202b4f5ee74e3d1300604967..ce6e299fe352e1d830437d075ab79ab4583d58ee 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -5,7 +5,7 @@ namespace car
 {
 
 EgoMotion::EgoMotion(ros::NodeHandle nh)
-    : messageOStream(nh)
+    : messageOStream(nh, "EgoMotion")
       , platoonControllerNotify()
       , cruiseControllerNotify()
 {
@@ -15,23 +15,19 @@ EgoMotion::EgoMotion(ros::NodeHandle nh)
 
 void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
 {
-    messageOStream << "EgoMotion recived new environmentData data ("
-                   << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
     distance = inMsg->distance;
     relSpeed = inMsg->relativeSpeed;
     computeAndNotify();
-    messageOStream << "EgoMotion done with environmentData data ("
-                   << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << MessageOStream::flush;
+
+    messageOStream.write("distance", distance).write("relSpeed", relSpeed);
 }
 
 void EgoMotion::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
 {
-    messageOStream << "EgoMotion recived new stmData data ("
-                   << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
     stmSpeed = inMsg->speed;
     computeAndNotify();
-    messageOStream << "EgoMotion done with stmData data ("
-                   << inMsg->speed << ", " << inMsg->angle << ")." << MessageOStream::flush;
+
+    messageOStream.write("stmSpeed", stmSpeed);
 }
 
 void EgoMotion::computeAndNotify()
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index d50083e2ebeef527406726127f33c0cb47a32ef3..d565355d4f70993d6bf7aef021326ad57615952c 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
@@ -61,7 +61,7 @@ platoonProtocol::NetworkInfo readNetworkInfo() {
 
 MainNode::MainNode()
     : nh()
-      , messageOStream(nh)
+      , messageOStream(nh, "MainNode")
       , c2cNet()
       , c2c(platoonProtocol::Vehicle::Role::FOLLOWER, c2cNet, readNetworkInfo())
       , pcNet()
@@ -81,8 +81,8 @@ MainNode::MainNode()
 
 void MainNode::onInit()
 {
-	messageOStream << "MainNode::onInit -- START";
-    messageOStream << "MainNode::onInit -- END";
+	messageOStream.write("onInit", "START");
+    messageOStream.write("onInit", "END");
 }
 
 }
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index eaf6d79ac8d2276eb01f30c31729425b2241ab5b..58c21b3b20ed094ffaa815f10eab88e00d3ab08d 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -11,7 +11,7 @@ PlatoonController::PlatoonController(ros::NodeHandle & nh,
                                      platoonProtocol::VehicleFacade & c2c,
                                      pc2car::CommandReceiver::Ptr pc,
                                      EgoMotion & egoMotion)
-    : messageOStream(nh)
+    : messageOStream(nh, "PlatoonController")
       , cruiseControllerNotify()
       , c2c(c2c)
       , pc(std::move(pc))
@@ -67,7 +67,8 @@ void PlatoonController::setupC2C()
 
 void PlatoonController::run()
 {
-    messageOStream << "PlatoonController was run." << MessageOStream::flush;
+    messageOStream.write("run", "");
+
     switch (currState.load())
     {
         case PlatoonState::ACC:
@@ -151,8 +152,9 @@ void PlatoonController::run_CACC_FV()
     // needs to be updated, so the new value can be pulled be CC
     platoonConfig = c2c.getPlatoonConfig(); // TODO <- updateC2CConfig();
 
-    messageOStream << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
-                   << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "run_CACC_FV"
+                   << MessageOStream::value << "inPlatoon: " << inPlatoon << "\nwantsPlatoon: " << wantsPlatoon
+                   << MessageOStream::flush;
 
     if (inPlatoon && wantsPlatoon)
     {
@@ -174,8 +176,9 @@ void PlatoonController::run_CACC_LV()
     bool inPlatoon = c2c.isPlatoonRunning();
     bool wantsPlatoon = pc->isPlatoonEnabled().get();
 
-    messageOStream << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
-                   << ", wantsPlatoon = " << wantsPlatoon << MessageOStream::flush;
+    messageOStream << MessageOStream::key << "run_CACC_LV"
+                   << MessageOStream::value << "inPlatoon: " << inPlatoon << "\nwantsPlatoon: " << wantsPlatoon
+                   << MessageOStream::flush;
 
     if (inPlatoon && wantsPlatoon)
     {
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index 6b362421fb5d1bf94e20cdf116db985aa6642ec1..e463b58cd0053a9050e4f343dada9811d52e231e 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -11,12 +11,12 @@ namespace car
 {
 
 MavLink::MavLink()
-    : messageOStream(nh)
+    : messageOStream(nh, "MavLink")
 {}
 
 void MavLink::onInit()
 {
-    messageOStream << "MavLink::onInit -- START" << MessageOStream::flush;
+    messageOStream.write("onInit", "START");
 
     stmData = nh.advertise<stmDataMsg>("stmData", 1);
     ccData = nh.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
@@ -29,9 +29,9 @@ void MavLink::onInit()
         [this]
         { onStmDataReceived(); },
         [this]
-        { messageOStream << "UART was closed!" << MessageOStream::flush; });
+        { messageOStream.write("ERROR", "UART was closed"); });
 
-    messageOStream << "MavLink::onInit -- END" << MessageOStream::flush;
+    messageOStream.write("onInit", "END");
 }
 
 void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
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 bac34fe89c6d3e9788bdff7ef603cc7072a18bc8..6cda3f7905a5d7e006a13337aa2e4404b0db2c5c 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/USS_SRF02.cpp
@@ -19,14 +19,14 @@ constexpr int USS_SRF02::DELAY;
 
 USS_SRF02::USS_SRF02(int devId)
 {
-    //fd = wiringPiI2CSetupInterface(DEVICE, devId);
-    //if (fd == -1)
-        //throw std::runtime_error{"Device not found!\n"};
+    fd = wiringPiI2CSetupInterface(DEVICE, devId);
+    if (fd == -1)
+        throw std::runtime_error{"Device not found!\n"};
 }
 
 int USS_SRF02::getDistance()
 {
-    //wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
+    wiringPiI2CWriteReg8(fd, COMMAND_REGISTER, RANGING_MODE_CM);
     usleep(DELAY * 1000);
-    //return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
+    return wiringPiI2CReadReg16(fd, RESULT_LOW_BYTE);
 }
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
index 2109e5a1ad2ff5301133b91f1b580747a0c0e4d7..3e8df4b9bb1b8755396d6c424bc3925dcb178125 100644
--- a/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
+++ b/modules/catkin_ws/src/car/src/ultrasonic/Ultrasonic.cpp
@@ -13,12 +13,14 @@ namespace car
 
 Ultrasonic::Ultrasonic()
     : USS_SRF02(DEVICE_ADDRESS1)
-      , messageOStream(nh)
+      , messageOStream(nh, "Ultrasonic")
 {}
 
+static std::size_t counter = 0;
+
 void Ultrasonic::onInit()
 {
-    messageOStream << "Ultrasonic::onInit -- START" << MessageOStream::flush;
+    messageOStream.write("onInit", "START");
 
     ussData = nh.advertise<ussDataMsg>("ussData", 1);
     sensorThread = std::thread(
@@ -33,6 +35,6 @@ void Ultrasonic::onInit()
             }
         });
 
-    messageOStream << "Ultrasonic::onInit -- END" << MessageOStream::flush;
+    messageOStream.write("onInit", "STOP");
 }
 }