From c99c923e28afd4983107a9321446b08dfd4e9a4d Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Thu, 17 May 2018 14:43:11 +0200
Subject: [PATCH] Fix dist dependence in PLC
---
.../src/car/include/mainNode/PlatoonController.h | 2 ++
.../src/car/src/mainNode/PlatoonController.cpp | 13 +++++++++----
2 files changed, 11 insertions(+), 4 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index b63c65bf..ab3bbe47 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 91b67b6e..7d2fa747 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();
--
GitLab