Implements PLC-logic

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

@@ -13,6 +13,7 @@
 #include "EgoMotion.h"
 #include "PlatoonState.h"
 #include <atomic>
+#include <chrono>
 
 namespace car
 {
@@ -30,21 +31,29 @@ public:
     
     Callback cruiseControllerNotify;
      
-    // TODO check these values !
     // TODO make values ATOMIC !
-    // these values need to be stored and atomic, since they will be pulled from other modules
-    PlatoonState curState = PlatoonState::CACC_FV;
-    float        desSpeed = 0.0;
-    platoonProtocol::PlatoonConfig platoonConfig;
+    // these value need to be stored and atomic, since they will be pulled from other modules
+    PlatoonState curState = PlatoonState::ACC;
+    
+    platoonProtocol::PlatoonConfig platoonConfig; // TODO needs to be removed, once C2C returns TimedValues
 
 private:
-
     platoonProtocol::VehicleFacade& c2c;
+    pc2car::CommandReceiver::Ptr pc;
+    EgoMotion& egoMotion;
+
     bool c2cAlive = false;
+    // void updateC2cConfig(); // once C2C returns TimedValues
     
-    pc2car::CommandReceiver::Ptr pc;
+    void updatePcConfig();
+    pc2car::TimedValue<platoonProtocol::PlatoonSpeed>          PS{0.0f};
+    pc2car::TimedValue<platoonProtocol::InnerPlatoonDistance> IPD{0.0f};
     
-    EgoMotion& egoMotion;
+    void updateDesSpeed();
+    pc2car::TimedValue<float> desSpeed{0.0f};
+    
+
+    void setupC2C();
     
     // METHODS
     void run_ACC();

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

@@ -4,6 +4,7 @@
 
 #include "../../include/mainNode/PlatoonController.h"
 #include "PlatoonProtocolLib/VehicleFacade.h"
+#include "PC2CarLib/TimedValue.h"
 #include <iostream>
 
 namespace car
@@ -12,58 +13,134 @@ namespace car
 PlatoonController::PlatoonController(platoonProtocol::VehicleFacade& c2c,
     pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
     : cruiseControllerNotify() 
-      , platoonConfig(c2c.getPlatoonConfig()) 
       , c2c(c2c)
       , pc(pc)
       , egoMotion(egoMotion)
+      , platoonConfig(c2c.getPlatoonConfig()) 
     {}
 
+void PlatoonController::updatePcConfig() {
+    pc2car::TimedValue<platoonProtocol::PlatoonSpeed>         otherPS  = pc->getPlatoonSpeed();
+    pc2car::TimedValue<platoonProtocol::InnerPlatoonDistance> otherIPD = pc->getInnerPlatoonDistance();
+    
+    if (  otherPS.getTimestamp() >  PS.getTimestamp() ) {  PS = otherPS; }
+    if ( otherIPD.getTimestamp() > IPD.getTimestamp() ) { IPD = otherIPD; }
 
-void PlatoonController::run()
-{
-  std::cout << "PlatoonController was run." << std::endl;
-  
-  switch (curState) {
-    case PlatoonState::ACC: {
-      run_ACC();
-      break;
-    }
-                     
-    case PlatoonState::CACC_FV: {
-      run_CACC_FV();
-      break;
-    }
-                     
-    case PlatoonState::CACC_LV: {
-      run_CACC_LV();
-      break;
+    cruiseControllerNotify();
+}
+
+void PlatoonController::updateDesSpeed() {
+    pc2car::TimedValue<float> otherDesSpeed = pc->getInnerPlatoonDistance();
+    if ( otherDesSpeed.getTimestamp() > desSpeed.getTimestamp() ) { desSpeed = otherDesSpeed; }
+    
+    cruiseControllerNotify();
+}
+
+void PlatoonController::setupC2C() {
+    bool wantsPlatoon = pc->isPlatoonEnabled().get();
+    
+    if ( c2cAlive ) { return; } // if C2C is already alive ... do nothing
+    if (!wantsPlatoon ) { return; } // if we dont want platoon ... 
+    
+    // get role according to distance
+    if ( egoMotion.getDistance() == std::numeric_limits<float>::infinity() ) {
+        c2c.reset(platoonProtocol::Vehicle::Role::LEADER);
+        c2c.setInnerPlatoonDistance(IPD.get());
+        c2c.setPlatoonSpeed(PS.get());
+    } else {
+        c2c.reset(platoonProtocol::Vehicle::Role::FOLLOWER);
     }
-  }
-  
+    c2c.createPlatoon();
+    c2cAlive = true;
 }
 
+void PlatoonController::run() {
+    std::cout << "PlatoonController was run." << std::endl;
+    switch (curState) {
+        case PlatoonState::ACC:     { run_ACC();     break; }
+        case PlatoonState::CACC_FV: { run_CACC_FV(); break; }
+        case PlatoonState::CACC_LV: { run_CACC_LV(); break; }
+    }
+}
 
 void PlatoonController::run_ACC() {
-   bool inPlatoon = c2c.isPlatoonRunning(); 
-   bool wantsPlatoon = pc->isPlatoonEnabled().get();
-   // platoonProtocol::PlatoonSpeed PS = pc.getPlatoonSpeed();
+    
+    bool wantsPlatoon = pc->isPlatoonEnabled().get();
+
+    if (!wantsPlatoon) {
+        if (c2cAlive) { c2c.leavePlatoon(); c2cAlive = false;}
+        updateDesSpeed();
+        return;
+    } 
+    
+    // wantsPlatoon
+    if (!c2cAlive) {
+        updateDesSpeed();
+        setupC2C();
+        return;
+    }
 
+    // c2cAlive + wantsPlatoon
+    if (c2c.isPlatoonRunning()) {
+        if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {
+            updatePcConfig();
+            curState =  PlatoonState::CACC_LV;
+        }
+        else {curState =  PlatoonState::CACC_FV;}
+        return;
+    }
+
+    // !inPlatoon + c2cAlive + wantsPlatoon
+    bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER;
+    bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity();
+    if ( isLeader != hasFiniteDistance ) { // role does not fit
+        c2c.leavePlatoon();
+        c2cAlive = false;
+        updateDesSpeed();
+        setupC2C();
+        return;
+    }
+
+    // role fits + !inPlatoon + c2cAlive + wantsPlatoon
+    if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {updatePcConfig();}
+    updateDesSpeed();
+    return;
 }
 
 void PlatoonController::run_CACC_FV() {
-   bool inPlatoon = c2c.isPlatoonRunning(); 
-   bool wantsPlatoon = pc->isPlatoonEnabled().get();
+    bool inPlatoon = c2c.isPlatoonRunning(); 
+    bool wantsPlatoon = pc->isPlatoonEnabled().get();
    
-   // Although this value will not be used in this method it still
-   // needs to be updated, so the new value can be pulled be CC
-   platoonConfig = c2c.getPlatoonConfig();
+    // Although this value will not be used in this method it still
+    // needs to be updated, so the new value can be pulled be CC
+    platoonConfig = c2c.getPlatoonConfig(); // TODO <- updateC2CConfig();
    
-   std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
-       << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+    std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
+        << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+
+    if (inPlatoon && wantsPlatoon) { cruiseControllerNotify(); return; } 
+    
+    if (inPlatoon && !wantsPlatoon) { c2c.leavePlatoon(); }
+    
+    c2cAlive = false;
+    curState = PlatoonState::ACC;
+    cruiseControllerNotify();
+    run_ACC();
+    return;
+}
+
+void PlatoonController::run_CACC_LV() {
+    bool inPlatoon = c2c.isPlatoonRunning(); 
+    bool wantsPlatoon = pc->isPlatoonEnabled().get();
+   
+    std::cout << "Running PlatoonController::run_CACC_LV: inPlatoon = " << inPlatoon
+        << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+    
 
     if (inPlatoon && wantsPlatoon) {
-       cruiseControllerNotify();
-       return;
+        updatePcConfig();
+        c2c.setInnerPlatoonDistance(IPD.get());
+        return;
     } 
     
     if (inPlatoon && !wantsPlatoon) {
@@ -74,11 +151,7 @@ void PlatoonController::run_CACC_FV() {
     curState = PlatoonState::ACC;
     cruiseControllerNotify();
     run_ACC();
-  return;
-}
-
-void PlatoonController::run_CACC_LV() {
-
+    return;
 }
 
 }