From 30945204d74d24f4b54463b29314619605b9e406 Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Fri, 20 Apr 2018 23:11:31 +0200
Subject: [PATCH] Implements PLC-logic
---
.../car/include/mainNode/PlatoonController.h | 25 ++-
.../car/src/mainNode/PlatoonController.cpp | 149 +++++++++++++-----
2 files changed, 128 insertions(+), 46 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index b21425c2..442508b0 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/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();
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index acc60f47..b5bf068a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/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;
}
}
--
GitLab