diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 261d606fa7809b5384a8bdbc120201dd7f862c70..c5d34c900e90134cf06ca6bca37026b2159f611d 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -16,7 +16,6 @@ void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inM
     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;
 }
@@ -26,14 +25,13 @@ 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;
+    prvSpeed = (stmSpeed.load() + relSpeed.load());
     cruiseControllerNotify();
 }