Implement CC

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

 #ifndef CRUISECONTROLLER_H
 #define CRUISECONTROLLER_H
 
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
 #include "PlatoonProtocolLib/VehicleFacade.h"
 #include "PlatoonController.h"
 #include "EgoMotion.h"
+#include "PlatoonState.h"
 
 namespace car
 {
@@ -12,7 +15,8 @@ class CruiseController
   public:
     CruiseController(platoonProtocol::VehicleFacade& c2c,
                      EgoMotion& egoMotion,
-                     PlatoonController& platoonController);
+                     PlatoonController& platoonController,
+                     ros::NodeHandle & nh);
     
     ~CruiseController(){};
 
@@ -22,6 +26,23 @@ class CruiseController
     platoonProtocol::VehicleFacade& c2c;
     EgoMotion& egoMotion;
     PlatoonController& platoonController;
+
+    float min_speed{0};
+    float max_speed{20};
+    float crash_time{5};
+    float stop_dist{20};
+
+    float eps{1e-9};
+    float speed{0};
+    float units_conv_fac{1};
+
+    float dist_pow {1.5};
+    float speed_pow{0.5};
+    
+    void run_ACC(float target_speed);
+    void run_CACC();
+    
+    ros::Publisher ccData;
 };
 }
 #endif

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

 #include "mainNode/CruiseController.h"
 
 #include "PlatoonProtocolLib/VehicleFacade.h"
+#include "PlatoonProtocolLib/Protocol.h"
+
+#include "car/ccDataMsg.h"
 
 #include <iostream>
+#include <algorithm>
 
 namespace car {
 
 CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c, 
                                    EgoMotion& egoMotion,
-                                   PlatoonController& platoonController)
+                                   PlatoonController& platoonController,
+                                   ros::NodeHandle& nh)
     : c2c(c2c)
       , egoMotion(egoMotion)
       , platoonController(platoonController)
-{ }
+    {
+        ccData = nh.advertise<ccDataMsg>("ccData", 1);
+    }
 
 void CruiseController::run() {
-    std::cout << "CruiseController was run." << std::endl;
+    switch (platoonController.curState) {
+        case PlatoonState::ACC: {
+            run_ACC(platoonController.getDesSpeed());
+            break;
+        }
+        case PlatoonState::CACC_LV: {
+            platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+            run_ACC(config.platoonSpeed);
+            c2c.setPlatoonSpeed(speed);
+            break;
+        }
+        case PlatoonState::CACC_FV: { run_CACC(); break; }
+    }
+}
+
+void CruiseController::run_ACC(float target_speed) {
+    
+    std::cout << "CruiseController::run_ACC("<< target_speed <<")" << std::endl;
+    float v_self = egoMotion.getOwnSpeed();
+    float d = egoMotion.getDistance();
+    float crash_dist = crash_time * v_self;
+    
+    if ( d < crash_dist ) {
+        std::cout << "in crash_dist" << std::endl;
+        speed = target_speed*(d - stop_dist)/crash_dist;
+    } else {
+        std::cout << "not in crash_dist" << std::endl;
+        speed = target_speed;
+    }
+    
+    speed = std::min(std::max(speed, min_speed), max_speed);
+    
+    ccDataMsg outMsg;
+    outMsg.speed = speed;
+    ccData.publish(outMsg);
+
+    return;
+}
+
+void CruiseController::run_CACC() {
+    
+    std::cout << "CruiseController::run_CACC" << std::endl;
+
+    platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+    float PS  = config.platoonSpeed;
+    float IPD = config.innerPlatoonDistance;
+    float vp  = egoMotion.getPrvSpeed();
+    float d   = egoMotion.getDistance();
+
+    if ( IPD < 1 ) { IPD = 1; }
+    if ( vp  < eps && vp > -eps ) {
+        if ( d < IPD ) { speed = 0; return; }
+        speed = units_conv_fac*0.5*(d-IPD)/IPD; // THIS IS DEPENDEND ON THE UNITS OF speed AND d!!!
+    } else {
+        speed = pow(d/IPD,dist_pow) * pow(PS/vp,speed_pow) * vp;
+    }
+
+    speed = std::min(std::max(speed, min_speed), max_speed);
+
+    ccDataMsg outMsg;
+    outMsg.speed = speed;
+    ccData.publish(outMsg);
+
+    return;
 }
 
+
 }

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

@@ -70,7 +70,7 @@ MainNode::MainNode(ros::NodeHandle & nh, std::string & name) :
     , egoMotion(nh)
     , platoonController(c2c, pc, egoMotion)
     , platoonControllerThread([this] { platoonController.run(); })
-    , cruiseController(c2c, egoMotion, platoonController)
+    , cruiseController(c2c, egoMotion, platoonController, nh)
     , cruiseControllerThread([this] { cruiseController.run(); })
 {
   egoMotion.cruiseControllerNotify = cruiseControllerNotify;
@@ -90,7 +90,7 @@ MainNode::MainNode() :
     , egoMotion(nh)
     , platoonController(c2c, pc, egoMotion)
     , platoonControllerThread([this] { platoonController.run(); })
-    , cruiseController(c2c, egoMotion, platoonController)
+    , cruiseController(c2c, egoMotion, platoonController, nh)
     , cruiseControllerThread([this] { cruiseController.run(); })
 {
   egoMotion.cruiseControllerNotify = cruiseControllerNotify;