-

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();
 }