diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 00f1970048235fb39ae1ddd792a5c5f3b787b36d..ff28f96119a14ae1f097def6ffe0bbfc6ab184ce 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -25,7 +25,7 @@ private:
     std::string name_;
 
     float distance;
-    float prevSpeed;
+    float relativeSpeed;
 
     ros::Publisher environmentData;
     ros::Subscriber ussData;
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index d398bd90b53bfcad75df7b82ec7396bfa0b6b7e6..ea796eb2d1fb4d1112c35c2b23f78d782014b2d3 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -5,8 +5,10 @@
 #include <ros/ros.h>
 
 #include <functional>
+#include <atomic>
 
 #include "car/environmentDataMsg.h"
+#include "car/stmDataMsg.h"
 
 namespace car
 {
@@ -22,9 +24,18 @@ class EgoMotion
     Callback cruiseControllerNotify;
     
   private:
+    std::atomic<float> distance{0};
+    std::atomic<float> stmSpeed{0};
+    std::atomic<float> relSpeed{0};
+    std::atomic<float> prvSpeed{0};
+    
     ros::Subscriber environmentData;
+    ros::Subscriber stmData;
+    
     void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
+    void stmDataCallback(const stmDataMsg::ConstPtr & inMsg);
     
+    void computeAndNotify();
 };
 }
 
diff --git a/modules/catkin_ws/src/car/include/mavLink/MavLink.h b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
index 836a23f018f79f5d311af37eb5aeb6da15099f57..857c542dea73edcfce69bbe7b738b6596b91cde1 100644
--- a/modules/catkin_ws/src/car/include/mavLink/MavLink.h
+++ b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
@@ -8,6 +8,8 @@
 #include "car/laneDataMsg.h"
 #include "car/rcEnabledMsg.h"
 
+#include "boost/thread.hpp"
+
 namespace car
 {
 	class MavLink : public nodelet::Nodelet
@@ -30,6 +32,9 @@ namespace car
       void ccDataCallback(const ccDataMsg::ConstPtr& inMsg);
       void laneDataCallback(const laneDataMsg::ConstPtr& inMsg);
       void rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg);
+      
+      
+      boost::thread main;
 	};
 }
 #endif
diff --git a/modules/catkin_ws/src/car/msg/environmentDataMsg.msg b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
index 8fb4625f1e311645dd1abfccfd3bf23721df5dc6..080ad908c347ceae5f59b4e3c31e7f8cfaecbb62 100644
--- a/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
+++ b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
@@ -1,2 +1,2 @@
 float32 distance
-float32 prevSpeed
+float32 relativeSpeed
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index df596bbc27333d315c357fcc3da38b717fa91562..8718a215ba057ff7b779c80441482643e229166e 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -15,7 +15,7 @@ namespace car
     nh_(nh), 
     name_(name),
     distance(0),
-    prevSpeed(0) {}
+    relativeSpeed(0) {}
   
 	Environment::Environment() {}
   
@@ -34,11 +34,11 @@ namespace car
   {
     // std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
     distance = inMsg->distance;
-    prevSpeed = 1.0;
+    relativeSpeed = 1.0;
     
     environmentDataMsg outMsg;
     outMsg.distance = distance;
-    outMsg.prevSpeed = prevSpeed;
+    outMsg.relativeSpeed = relativeSpeed;
     
     environmentData.publish(outMsg);
   }
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 8ad351c01ab99c10c55a71646027a406b5bf3b9c..261d606fa7809b5384a8bdbc120201dd7f862c70 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -8,11 +8,32 @@ EgoMotion::EgoMotion(ros::NodeHandle nh)
       , cruiseControllerNotify()
 {
     environmentData = nh.subscribe("environmentData", 1, &EgoMotion::environmentDataCallback, this);
+    stmData = nh.subscribe("stmData", 1, &EgoMotion::stmDataCallback, this);
 }
 
 void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
 {
-    std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+    std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+    distance = inMsg->distance;
+    relSpeed = inMsg->relativeSpeed;
+    sleep(3);
+    computeAndNotify();
+    std::cout << "EgoMotion done with environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+}
+
+
+void EgoMotion::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
+{
+    std::cout << "EgoMotion recived new stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+    stmSpeed = inMsg->speed;
+    sleep(1);
+    computeAndNotify();
+    std::cout << "EgoMotion done with stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+}
+
+void EgoMotion::computeAndNotify()
+{
+    prvSpeed = stmSpeed + relSpeed;
     cruiseControllerNotify();
 }
 
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index 6a735c5f9c5f82119173d34136820ae18e52b56e..dcd99e90ad75e61dbf5ca4d621451f2b1a03dc2e 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -28,6 +28,20 @@ namespace car
 	  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");
 	}