diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 26947910a159c1e9663baba339c243ad0efb48b5..c53d73dff809e70ab71951b9488022fa3aeb8b72 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/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)
diff --git a/modules/catkin_ws/src/car/include/mainNode/C2CBase.h b/modules/catkin_ws/src/car/include/mainNode/C2CBase.h
new file mode 100644
index 0000000000000000000000000000000000000000..97f17adedfb0543ddf6cb9f4db456ffd0f2b3dbf
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/mainNode/C2CBase.h
@@ -0,0 +1,40 @@
+#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
diff --git a/modules/catkin_ws/src/car/include/mainNode/MainNode.h b/modules/catkin_ws/src/car/include/mainNode/MainNode.h
index 60a4c812b5b27fc6d0745b8fab39b9e4141020ee..ff91be4d5340a148bb3509ae3ac000c476707a74 100644
--- a/modules/catkin_ws/src/car/include/mainNode/MainNode.h
+++ b/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;
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index 7f858bf2303aa8869bde325224760c4ca2bf2a6a..4191b18208b688fbb38ad12222e32710b8441c9c 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/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
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonState.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonState.h
new file mode 100644
index 0000000000000000000000000000000000000000..99875f71af74c6507f9bce4e4c65fc370309a5ab
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonState.h
@@ -0,0 +1,6 @@
+#ifndef PLATOONSTATE_H
+#define PLATOONSTATE_H
+namespace car {
+ enum class PlatoonState { ACC, CACC_FV, CACC_LV };
+}
+#endif // PLATOONSTATE_H
diff --git a/modules/catkin_ws/src/car/src/mainNode/C2CBase.cpp b/modules/catkin_ws/src/car/src/mainNode/C2CBase.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..64c3b3e0f2230b6903af55ee1eb4a09e732b7446
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/mainNode/C2CBase.cpp
@@ -0,0 +1,32 @@
+#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;
+ }
+
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
index 29e50dfc3be6ce8cbc171539376f423787e50351..04aba283a1105c29bee6133fca3cfbd773d0ce44 100644
--- a/modules/catkin_ws/src/car/src/mainNode/MainNode.cpp
+++ b/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)
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index 9eaf82972afe498ca085ab148c007e5c09541131..77b15bac612f89763b97cd9b167418772d5a5ba1 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/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() {