diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 8aec39519a0f1d3a27ba60e1da5f2265528f1aef..4bdfad45b594abc338948b8e00c1a54624dd2bb0 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -31,6 +31,8 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include
   LIBRARIES main_node
+  LIBRARIES environment
+  LIBRARIES ultrasonic
   CATKIN_DEPENDS roscpp std_msgs nodelet rospy #message_runtime
 )
 
@@ -43,9 +45,19 @@ 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})
+
 install(
   TARGETS
     main_node
+    environment
+    ultrasonic
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/modules/catkin_ws/src/car/include/environment/environment.h b/modules/catkin_ws/src/car/include/environment/environment.h
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..29d948e0b796c5ea8a6ace0da63bec3f973acafc 100644
--- a/modules/catkin_ws/src/car/include/environment/environment.h
+++ b/modules/catkin_ws/src/car/include/environment/environment.h
@@ -0,0 +1,32 @@
+#ifndef ENVIRONMENT_H
+#define ENVIRONMENT_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "car/ussDataMsg.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);
+	};
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/mainNode.h b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
index 46ab0157f246fb581175f4173065a639f17383f6..1c3b73f2cb6c77699da12c3570570372ac4b5578 100644
--- a/modules/catkin_ws/src/car/include/mainNode/mainNode.h
+++ b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
@@ -4,6 +4,7 @@
 #include <nodelet/nodelet.h>
 #include <ros/ros.h>
 #include "boost/thread.hpp"
+#include "car/environmentDataMsg.h" 
 
 namespace car
 {
@@ -19,6 +20,9 @@ namespace car
 			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
index 068f3cd51933dc082045e7c22edfa946d645dcf9..ec74c6bcb9f233d3b5caa6f47e51a77e8f086fef 100644
--- a/modules/catkin_ws/src/car/launch/fullstart.launch
+++ b/modules/catkin_ws/src/car/launch/fullstart.launch
@@ -4,6 +4,8 @@
   </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"/>
 </launch>
 
 <!--
diff --git a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
index 6715ecbb767d5753cdb12625c52301185064bb3c..0767fe032dc22b7bcfd711bd95ceb9a0112f7344 100644
--- a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
+++ b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
@@ -13,3 +13,23 @@
   </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>
diff --git a/modules/catkin_ws/src/car/src/environment/environment.cpp b/modules/catkin_ws/src/car/src/environment/environment.cpp
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..63934b43a8f670783aa37f3e9512c2270da48625 100644
--- a/modules/catkin_ws/src/car/src/environment/environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/environment.cpp
@@ -0,0 +1,47 @@
+#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);
+
+
+    		// CODE GOES HERE
+		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);
+  }
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
index 872ede66ce8a0231419d13268ec6bac52fa03e8b..ad5a8b1a17b6177239065dfac39341232980cdf2 100644
--- a/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
@@ -23,9 +23,16 @@ namespace car
 	{
 		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");
+	}
+}