-

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

@@ -121,7 +121,12 @@ MainNode::MainNode()
     egoMotion.setPlatoonControllerNotify(platoonControllerNotify);
     platoonController.setCruiseControllerNotify(cruiseControllerNotify);
     pc->receiveCommands([this] (auto commandCode) {platoonControllerThread.notify();} );
-    c2c.setCallback( [this] {platoonControllerThread.notify();} );
+    c2c.setCallback(
+        [this]
+        {
+            messageOStream.write("MSG", "C2C CALLBACK");
+            platoonControllerThread.notify();
+        });
     c2c.addVehicleEndpoints(readVehicleEndpoints());
 }
 

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

@@ -54,12 +54,14 @@ void PlatoonController::setupC2C()
     if (egoMotion.getDistance() <= maxPlatoonRange)
     {
         c2c.reset(platoonProtocol::Vehicle::Role::FOLLOWER);
+        messageOStream.write("MSG", "I'll become follower");
     }
     else
     {
         c2c.reset(platoonProtocol::Vehicle::Role::LEADER);
         c2c.setInnerPlatoonDistance(IPD.get());
         c2c.setPlatoonSpeed(PS.get());
+        messageOStream.write("MSG", "I'll become leader");
     }
     c2c.createPlatoon();
     c2cAlive = true;
@@ -129,6 +131,7 @@ void PlatoonController::run_ACC()
     bool inPlatoonRange = egoMotion.getDistance() <= maxPlatoonRange;
     if (isLeader == inPlatoonRange)
     { // role does not fit
+        messageOStream.write("MSG", "inconsistent state");
         c2c.leavePlatoon();
         c2cAlive = false;
         updateDesSpeed();