diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h index b63c65bf8530332bb694e6040204f05a55dd6014..ab3bbe47d2aaa4f5520ed352efba30657e4fd8b6 100644 --- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h +++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h @@ -43,6 +43,8 @@ public: { return platoonConfig.getNonAtomicCopy(); } private: + float maxPlatoonRange = 250.0; // cm + MessageOStream messageOStream; Callback cruiseControllerNotify; platoonProtocol::VehicleFacade & c2c; diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp index 91b67b6ed77801a714978002a629747a5655cd6e..7d2fa7476de9c11d9bece30f6b39361412f4a930 100644 --- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp +++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp @@ -67,6 +67,7 @@ void PlatoonController::setupC2C() void PlatoonController::run() { + // messageOStream.write("MSG", "RUUUUUUUUUUUUUUN!"); switch (currState.load()) { case PlatoonState::ACC: @@ -89,8 +90,9 @@ void PlatoonController::run() void PlatoonController::run_ACC() { - messageOStream.write("run - ACC", ""); bool wantsPlatoon = pc->isPlatoonEnabled().get(); + messageOStream.write("wantsPlatoon",wantsPlatoon); + messageOStream.write("c2cAlive",c2cAlive); if (!wantsPlatoon) { @@ -111,6 +113,7 @@ void PlatoonController::run_ACC() return; } + messageOStream.write("c2c.isPlatoonRunning",c2c.isPlatoonRunning()); // c2cAlive + wantsPlatoon if (c2c.isPlatoonRunning()) { @@ -126,7 +129,9 @@ void PlatoonController::run_ACC() // !inPlatoon + c2cAlive + wantsPlatoon bool isLeader = c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER; - bool hasFiniteDistance = egoMotion.getDistance() < std::numeric_limits<float>::infinity(); + bool inPlatoonRange = egoMotion.getDistance() < maxPlatoonRange; + messageOStream.write("isLeader",isLeader); + messageOStream.write("inPlatoonRange",inPlatoonRange); if (isLeader == inPlatoonRange) { // role does not fit c2c.leavePlatoon(); @@ -144,7 +149,7 @@ void PlatoonController::run_ACC() void PlatoonController::run_CACC_FV() { - messageOStream.write("run - FV", ""); + messageOStream.write("run", "FV"); bool inPlatoon = c2c.isPlatoonRunning(); bool wantsPlatoon = pc->isPlatoonEnabled().get(); @@ -173,7 +178,7 @@ void PlatoonController::run_CACC_FV() void PlatoonController::run_CACC_LV() { - messageOStream.write("run - LV", ""); + messageOStream.write("run", "LV"); bool inPlatoon = c2c.isPlatoonRunning(); bool wantsPlatoon = pc->isPlatoonEnabled().get();