-

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

@@ -51,15 +51,15 @@ void PlatoonController::setupC2C()
     { return; } // if we dont want platoon ...
 
     // get role according to distance
-    if (egoMotion.getDistance() == std::numeric_limits<float>::infinity())
+    if (egoMotion.getDistance() <= maxPlatoonRange)
     {
-        c2c.reset(platoonProtocol::Vehicle::Role::LEADER);
-        c2c.setInnerPlatoonDistance(IPD.get());
-        c2c.setPlatoonSpeed(PS.get());
+        c2c.reset(platoonProtocol::Vehicle::Role::FOLLOWER);
     }
     else
     {
-        c2c.reset(platoonProtocol::Vehicle::Role::FOLLOWER);
+        c2c.reset(platoonProtocol::Vehicle::Role::LEADER);
+        c2c.setInnerPlatoonDistance(IPD.get());
+        c2c.setPlatoonSpeed(PS.get());
     }
     c2c.createPlatoon();
     c2cAlive = true;
@@ -126,7 +126,7 @@ void PlatoonController::run_ACC()
 
     // !inPlatoon + c2cAlive + wantsPlatoon
     bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
-    bool inPlatoonRange = egoMotion.getDistance() < maxPlatoonRange;
+    bool inPlatoonRange = egoMotion.getDistance() <= maxPlatoonRange;
     messageOStream.write("isLeader", isLeader);
     messageOStream.write("inPlatoonRange", inPlatoonRange);
     if (isLeader == inPlatoonRange)