-

modules/catkin_ws/src/car/include/mainNode/PlatoonController.h

@@ -46,7 +46,7 @@ private:
     float maxPlatoonRange = 100.0; // cm
     
     MessageOStream messageOStream;
-    Callback cruiseControllerNotify;
+    Callback cruiseControllerNotify = [] {};
     platoonProtocol::VehicleFacade & c2c;
     pc2car::CommandReceiver::Ptr pc;
     EgoMotion & egoMotion;

modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp

@@ -111,9 +111,9 @@ void PlatoonController::run_ACC()
     }
 
     // c2cAlive + wantsPlatoon
-    messageOStream.write("MSG", "c2cAlive + wantsPlatoon");
     if (c2c.isPlatoonRunning())
     {
+        messageOStream.write("MSG", "platoon is running");
         if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER)
         {
             updatePcConfig();
@@ -127,6 +127,8 @@ void PlatoonController::run_ACC()
     // !inPlatoon + c2cAlive + wantsPlatoon
     bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
     bool inPlatoonRange = egoMotion.getDistance() < maxPlatoonRange;
+    messageOStream.write("isLeader", isLeader);
+    messageOStream.write("inPlatoonRange", inPlatoonRange);
     if (isLeader == inPlatoonRange)
     { // role does not fit
         c2c.leavePlatoon();