Fix dist dependence in PLC

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

@@ -43,6 +43,8 @@ public:
     { return platoonConfig.getNonAtomicCopy(); }
 
 private:
+    float maxPlatoonRange = 250.0; // cm
+    
     MessageOStream messageOStream;
     Callback cruiseControllerNotify;
     platoonProtocol::VehicleFacade & c2c;

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

@@ -67,6 +67,7 @@ void PlatoonController::setupC2C()
 
 void PlatoonController::run()
 {
+    // messageOStream.write("MSG", "RUUUUUUUUUUUUUUN!");
     switch (currState.load())
     {
         case PlatoonState::ACC:
@@ -89,8 +90,9 @@ void PlatoonController::run()
 
 void PlatoonController::run_ACC()
 {
-    messageOStream.write("run - ACC", "");
     bool wantsPlatoon = pc->isPlatoonEnabled().get();
+    messageOStream.write("wantsPlatoon",wantsPlatoon);
+    messageOStream.write("c2cAlive",c2cAlive);
 
     if (!wantsPlatoon)
     {
@@ -111,6 +113,7 @@ void PlatoonController::run_ACC()
         return;
     }
 
+    messageOStream.write("c2c.isPlatoonRunning",c2c.isPlatoonRunning());
     // c2cAlive + wantsPlatoon
     if (c2c.isPlatoonRunning())
     {
@@ -126,7 +129,9 @@ void PlatoonController::run_ACC()
 
     // !inPlatoon + c2cAlive + wantsPlatoon
     bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
-    bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity();
+    bool inPlatoonRange = egoMotion.getDistance() < maxPlatoonRange;
+    messageOStream.write("isLeader",isLeader);
+    messageOStream.write("inPlatoonRange",inPlatoonRange);
     if (isLeader == inPlatoonRange)
     { // role does not fit
         c2c.leavePlatoon();
@@ -144,7 +149,7 @@ void PlatoonController::run_ACC()
 
 void PlatoonController::run_CACC_FV()
 {
-    messageOStream.write("run - FV", "");
+    messageOStream.write("run", "FV");
     bool inPlatoon = c2c.isPlatoonRunning();
     bool wantsPlatoon = pc->isPlatoonEnabled().get();
 
@@ -173,7 +178,7 @@ void PlatoonController::run_CACC_FV()
 
 void PlatoonController::run_CACC_LV()
 {
-    messageOStream.write("run - LV", "");
+    messageOStream.write("run", "LV");
     bool inPlatoon = c2c.isPlatoonRunning();
     bool wantsPlatoon = pc->isPlatoonEnabled().get();