From 4d19a4c3726dd95d2062dabbf1415b88deb53afc Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Tue, 27 Mar 2018 16:21:38 +0200
Subject: [PATCH] tmp commit; not working
---
modules/catkin_ws/src/PC/CMakeLists.txt | 4 +-
.../car/include/mainNode/PlatoonController.h | 18 +++++++
.../car/src/mainNode/PlatoonController.cpp | 54 ++++++++++++++++++-
3 files changed, 72 insertions(+), 4 deletions(-)
diff --git a/modules/catkin_ws/src/PC/CMakeLists.txt b/modules/catkin_ws/src/PC/CMakeLists.txt
index 226a1708..5f14c372 100644
--- a/modules/catkin_ws/src/PC/CMakeLists.txt
+++ b/modules/catkin_ws/src/PC/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.9)
+cmake_minimum_required(VERSION 3.5.1)
project(PC)
set(CMAKE_CXX_STANDARD 14)
@@ -12,4 +12,4 @@ set(SOURCE_FILES
add_executable(PC ${SOURCE_FILES})
-target_link_libraries(PC ${Boost_LIBRARIES})
\ No newline at end of file
+target_link_libraries(PC ${Boost_LIBRARIES})
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index 982f099b..d68df79b 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -8,12 +8,15 @@
#include <functional>
#include "PlatoonProtocolLib/Vehicle.h"
+#include "PlatoonProtocolLib/Protocol.h"
#include "PC2CarLib/CommandReceiver.h"
#include "EgoMotion.h"
namespace car
{
+enum class State { ACC, CACC_FV, CACC_LV };
+
class PlatoonController
{
public:
@@ -24,6 +27,7 @@ public:
EgoMotion& egoMotion);
void run();
+
Callback cruiseControllerNotify;
private:
@@ -31,7 +35,21 @@ private:
platoonProtocol::Vehicle::Ptr c2c;
pc2car::CommandReceiver::Ptr pc;
EgoMotion& egoMotion;
+
+ State curState = State::CACC_FV;
+
+ bool wantsPlatoon = false;
+ // TODO check these values!
+ PlatoonConfig platoonConfig {50.0, 0.0}; // IPD=50.0 PS=0.0
+ float desSpeed = 0.0;
+
+ // METHODS
+
+ void run_ACC();
+ void run_CACC_FV();
+ void run_CACC_LV();
+
};
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index 3231cc5b..2adb912f 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -18,8 +18,58 @@ PlatoonController::PlatoonController(platoonProtocol::Vehicle::Ptr c2c,
void car::PlatoonController::run()
{
- // TODO: implementation
- std::cout << "PlatoonController was run." << std::endl;
+ std::cout << "PlatoonController was run." << std::endl;
+
+ switch (curState) {
+ case State::ACC: {
+ run_ACC();
+ break;
+ }
+
+ case State::CACC_FV: {
+ run_CACC_FV();
+ break;
+ }
+
+ case State::CACC_LV: {
+ run_CACC_LV();
+ break;
+ }
+ }
+
+}
+
+
+void car::PlatoonController::run_ACC() {
+
+}
+
+void car::PlatoonController::run_CACC_FV() {
+ bool inPlatoon = c2c->isPlatoonRunning();
+ 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();
+ }
+
+ c2c
+ curState = State::ACC;
+ cruiseControllerNotify();
+ run_ACC();
+}
+
+void car::PlatoonController::run_CACC_LV() {
+
}
}
--
GitLab