Adds C2CBase as fassade

modules/catkin_ws/src/car/CMakeLists.txt

@@ -55,12 +55,16 @@ set(MAIN_NODE_SOURCE_FILES
         include/mainNode/EgoMotion.h
         include/mainNode/NotifiableThread.h
         include/mainNode/PlatoonController.h
+        include/mainNode/PlatoonState.h      # only header for enum type
         include/mainNode/CruiseController.h
+        include/mainNode/C2CBase.h
         src/mainNode/MainNode.cpp
         src/mainNode/EgoMotion.cpp
         src/mainNode/NotifiableThread.cpp
         src/mainNode/PlatoonController.cpp
-        src/mainNode/CruiseController.cpp)
+        src/mainNode/CruiseController.cpp
+        src/mainNode/C2CBase.cpp
+   )
 
 set(LOCAL_INSTALL_DIR ${CMAKE_CURRENT_LIST_DIR}/../../install)
 

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

+#ifndef C2CBASE_H
+#define C2CBASE_H
+
+#include "PlatoonProtocolLib/Vehicle.h"
+#include "PlatoonProtocolLib/LeaderVehicle.h"
+#include "PlatoonProtocolLib/FollowerVehicle.h"
+#include "PlatoonProtocolLib/Protocol.h"
+#include "PlatoonState.h"
+
+namespace car
+{
+  
+  
+class C2CBase
+{
+    friend class PlatoonController; // only the PlatoonControler should be able to use the swap functions below!
+    public:
+        C2CBase();
+      
+    private:
+        PlatoonState curState = PlatoonState::ACC;
+      
+        networking::Networking c2cNetLeader;   // thread  + event queue
+        networking::Networking c2cNetFollower; // thread  + event queue
+      
+        platoonProtocol::LeaderVehicle::Ptr   c2cLeader;
+        platoonProtocol::FollowerVehicle::Ptr c2cFollower;
+      
+        void swap2LV();
+        void swap2FV();
+        void swap2ACC();
+
+        bool isPlatoonRunning();
+        platoonProtocol::PlatoonConfig getPlatoonConfig();
+
+};
+
+}
+
+#endif // C2CBASE_H

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

@@ -7,7 +7,8 @@
 
 #include "NotifiableThread.h"
 #include "NetworkingLib/Networking.h"
-#include "PlatoonProtocolLib/Vehicle.h"
+// #include "PlatoonProtocolLib/Vehicle.h"
+#include "C2CBase.h"
 #include "PC2CarLib/CommandReceiver.h"
 
 #include "PlatoonController.h"
@@ -31,10 +32,11 @@ private:
     ros::NodeHandle nh;
     std::string name;
     
-    networking::Networking c2cNet; // thread  + event queue
+    // OLD needs to be removed after test! networking::Networking c2cNet; // thread  + event queue
+    // OLD needs to be removed after test! platoonProtocol::Vehicle::Ptr c2c;
+    C2CBase c2c;
+     
     networking::Networking pcNet; // thread  + event queue
-    
-    platoonProtocol::Vehicle::Ptr c2c;
     pc2car::CommandReceiver::Ptr pc;
 
     EgoMotion egoMotion;

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

@@ -7,22 +7,22 @@
 
 #include <functional>
 
-#include "PlatoonProtocolLib/Vehicle.h"
+// #include "PlatoonProtocolLib/Vehicle.h"
+#include "C2CBase.h"
 #include "PlatoonProtocolLib/Protocol.h"
 #include "PC2CarLib/CommandReceiver.h"
 #include "EgoMotion.h"
+#include "PlatoonState.h"
 
 namespace car
 {
 
-enum class State { ACC, CACC_FV, CACC_LV };
-
 class PlatoonController
 {
 public:
     using Callback = std::function<void()>;
     
-    PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+    PlatoonController(C2CBase& c2c,
                       pc2car::CommandReceiver::Ptr pc,
                       EgoMotion& egoMotion);
       
@@ -32,16 +32,16 @@ public:
 
 private:
 
-    platoonProtocol::Vehicle::Ptr c2c;
+    C2CBase& c2c;
     pc2car::CommandReceiver::Ptr pc;
     EgoMotion& egoMotion;
     
-    State curState = State::CACC_FV;
+    // TODO check these values!
+    PlatoonState curState = PlatoonState::CACC_FV;
 
     bool wantsPlatoon = false;
 
     // TODO check these values!
-    platoonProtocol::PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
     float desSpeed = 0.0;
 
     // METHODS

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

+#ifndef PLATOONSTATE_H
+#define PLATOONSTATE_H
+namespace car {
+    enum class PlatoonState { ACC, CACC_FV, CACC_LV };
+}
+#endif // PLATOONSTATE_H

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

+#include "../../include/mainNode/C2CBase.h"
+
+namespace car {
+
+    C2CBase::C2CBase(){}
+
+    void C2CBase::swap2LV()
+    {
+        curState = PlatoonState::CACC_LV;        
+    }
+    
+    void C2CBase::swap2FV()
+    {
+        curState = PlatoonState::CACC_FV;        
+    }
+
+    void C2CBase::swap2ACC()
+    {
+        curState = PlatoonState::ACC;        
+    }
+    
+    bool C2CBase::isPlatoonRunning()
+    {
+        std::cout << "C2CBase::isRunning" << std::endl;
+    }
+    
+    platoonProtocol::PlatoonConfig C2CBase::getPlatoonConfig()
+    {
+        std::cout << "C2CBase::getPlatoonConfig" << std::endl;
+    }
+    
+}

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

@@ -9,9 +9,10 @@ namespace car
 MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
     nh(nh)  
     , name(name)
-    , c2cNet()
-    , pcNet()
+    // OLD needs to be removed after test! , c2cNet()
+    // OLD needs to be removed after test! , c2c()
     , c2c()
+    , pcNet()
     , pc(pc2car::CommandReceiver::create(pcNet))
     , egoMotion(nh)
     , platoonController(c2c, pc, egoMotion)
@@ -28,9 +29,10 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
 MainNode::MainNode() : 
     nh()  
     , name()
-    , c2cNet()
-    , pcNet()
+    // OLD needs to be removed after test! , c2cNet()
+    // OLD needs to be removed after test! , c2c()
     , c2c()
+    , pcNet()
     , pc(pc2car::CommandReceiver::create(pcNet))
     , egoMotion(nh)
     , platoonController(c2c, pc, egoMotion)

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

@@ -9,7 +9,7 @@
 namespace car
 {
 
-PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
+PlatoonController::PlatoonController(C2CBase& c2c,
     pc2car::CommandReceiver::Ptr pc, EgoMotion& egoMotion)
     : c2c(c2c)
       , pc(pc)
@@ -22,17 +22,17 @@ void car::PlatoonController::run()
   std::cout << "PlatoonController was run." << std::endl;
   
   switch (curState) {
-    case State::ACC: {
+    case PlatoonState::ACC: {
       run_ACC();
       break;
     }
                      
-    case State::CACC_FV: {
+    case PlatoonState::CACC_FV: {
       run_CACC_FV();
       break;
     }
                      
-    case State::CACC_LV: {
+    case PlatoonState::CACC_LV: {
       run_CACC_LV();
       break;
     }
@@ -46,27 +46,29 @@ void car::PlatoonController::run_ACC() {
 }
 
 void car::PlatoonController::run_CACC_FV() {
-   bool inPlatoon = c2c->isPlatoonRunning(); 
-   platoonConfig = std::dynamic_pointer_cast<platoonProtocol::FollowerVehicle>(c2c)->getPlatoonConfig();
+   bool inPlatoon = c2c.isPlatoonRunning(); 
+   platoonProtocol::PlatoonConfig platoonConfig = c2c.getPlatoonConfig();
+   
    wantsPlatoon = pc->isPlatoonEnabled().get();
    
    
    std::cout << "Running PlatoonController::run_CACC_FV: inPlatoon = " << inPlatoon
      << ", wantsPlatoon = " << wantsPlatoon << std::endl;
 
-   if (inPlatoon && wantsPlatoon) {
-    cruiseControllerNotify();
-    return;
-   } 
-   
-   if (inPlatoon && !wantsPlatoon) {
-     c2c->leavePlatoon();
-   }
-   
-   // TODO: c2c->reset()
-   curState = State::ACC;
-   cruiseControllerNotify();
-   run_ACC();
+   // if (inPlatoon && wantsPlatoon) {
+   //  cruiseControllerNotify();
+   //  return;
+   // } 
+   // 
+   // if (inPlatoon && !wantsPlatoon) {
+   //   c2c->leavePlatoon();
+   // }
+   // 
+   // // TODO: c2c->reset()
+   // curState = PlatoonState::ACC;
+   // cruiseControllerNotify();
+   // run_ACC();
+  return;
 }
 
 void car::PlatoonController::run_CACC_LV() {