Reimplements PlatoonController::run_CACC_FV

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

@@ -12,6 +12,7 @@
 #include "PC2CarLib/CommandReceiver.h"
 #include "EgoMotion.h"
 #include "PlatoonState.h"
+#include <atomic>
 
 namespace car
 {
@@ -28,23 +29,24 @@ public:
     void run();
     
     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;
 
 private:
 
     platoonProtocol::VehicleFacade& c2c;
+    bool c2cAlive = false;
+    
     pc2car::CommandReceiver::Ptr pc;
+    
     EgoMotion& egoMotion;
     
-    // TODO check these values!
-    PlatoonState curState = PlatoonState::CACC_FV;
-
-    bool wantsPlatoon = false;
-
-    // TODO check these values!
-    float desSpeed = 0.0;
-
     // METHODS
-
     void run_ACC();
     void run_CACC_FV();
     void run_CACC_LV();

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

@@ -11,10 +11,12 @@ namespace car
 
 PlatoonController::PlatoonController(platoonProtocol::VehicleFacade& c2c,
     pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
-    : c2c(c2c)
+    : cruiseControllerNotify() 
+      , platoonConfig(c2c.getPlatoonConfig()) 
+      , c2c(c2c)
       , pc(pc)
       , egoMotion(egoMotion)
-      , cruiseControllerNotify() {}
+    {}
 
 
 void car::PlatoonController::run()
@@ -42,32 +44,33 @@ void car::PlatoonController::run()
 
 
 void car::PlatoonController::run_ACC() {
+   bool inPlatoon = c2c.isPlatoonRunning(); 
+   bool wantsPlatoon = pc->isPlatoonEnabled().get();
+   // platoonProtocol::PlatoonSpeed PS = pc.getPlatoonSpeed();
 
 }
 
 void car::PlatoonController::run_CACC_FV() {
    bool inPlatoon = c2c.isPlatoonRunning(); 
    platoonProtocol::PlatoonConfig platoonConfig = c2c.getPlatoonConfig();
-   
-   wantsPlatoon = pc->isPlatoonEnabled().get();
-   
+   bool wantsPlatoon = pc->isPlatoonEnabled().get();
    
    std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
-     << ", wantsPlatoon = " << wantsPlatoon << std::endl;
+       << ", wantsPlatoon = " << wantsPlatoon << std::endl;
 
-   // if (inPlatoon && wantsPlatoon) {
-   //  cruiseControllerNotify();
-   //  return;
-   // } 
-   // 
-   // if (inPlatoon && !wantsPlatoon) {
-   //   c2c->leavePlatoon();
-   // }
-   // 
-   // // TODO: c2c->reset()
-   // curState = PlatoonState::ACC;
-   // cruiseControllerNotify();
-   // run_ACC();
+    if (inPlatoon && wantsPlatoon) {
+       cruiseControllerNotify();
+       return;
+    } 
+    
+    if (inPlatoon && !wantsPlatoon) {
+        c2c.leavePlatoon();
+    }
+    
+    c2cAlive = false;
+    curState = PlatoonState::ACC;
+    cruiseControllerNotify();
+    run_ACC();
   return;
 }