From 2af59258c2adeb238b1d813863c35393f051e45e Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Thu, 3 May 2018 12:28:45 +0200
Subject: [PATCH] added TimedValue to PlatoonProtocol
---
.../include/NetworkingLib/TimedValue.h | 58 +++++++++++++------
.../catkin_ws/src/PC2CarLib/CMakeLists.txt | 9 ---
.../PlatoonProtocolLib/FollowerVehicle.h | 2 +-
.../include/PlatoonProtocolLib/Vehicle.h | 3 +-
.../PlatoonProtocolLib/VehicleFacade.h | 2 +-
.../src/FollowerVehicle.cpp | 6 +-
.../PlatoonProtocolLib/src/LeaderVehicle.cpp | 6 +-
.../PlatoonProtocolLib/src/VehicleFacade.cpp | 2 +-
.../PlatoonProtocolLib/test/FollowerTest.cpp | 4 +-
.../PlatoonProtocolLib/test/TestScenarios.cpp | 8 +--
.../include/VeloxProtocolLib/Connection.h | 2 -
.../car/include/mainNode/PlatoonController.h | 11 ++--
.../src/car/src/mainNode/CruiseController.cpp | 10 +---
.../car/src/mainNode/PlatoonController.cpp | 13 ++---
14 files changed, 70 insertions(+), 66 deletions(-)
diff --git a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
index 10cbe19b..2107a0b2 100644
--- a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
+++ b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h
@@ -23,29 +23,34 @@ public:
: timestamp(Timestamp::min())
{}
- TimedValue(const T & val)
+ explicit TimedValue(const T & val)
{ set(val); }
TimedValue(const T & val, const Timestamp & timestamp)
- : val(val), timestamp(timestamp)
- {}
-
- T get() const
- { return val; }
+ { set(val, timestamp); }
- void set(const T & val)
+ void set(const T & val, Timestamp timestamp)
{
this->val = val;
- timestamp = networking::time::now();
+ timestamp = timestamp;
}
- Timestamp getTimestamp() const
- { return timestamp; }
+ void set(const T & val)
+ { set(val, networking::time::now()); }
+
+ void set(const TimedValue & val)
+ { set(val.get(), val.getTimestamp()); }
TimedValue & operator=(const T & val)
{ set(val); }
- explicit operator T() const
+ T get() const noexcept
+ { return val; }
+
+ Timestamp getTimestamp() const noexcept
+ { return timestamp; }
+
+ explicit operator T() const noexcept
{ return get(); }
private:
@@ -63,20 +68,38 @@ public:
: timestamp(Timestamp::min())
{}
- TimedAtomicValue(const T & val)
+ explicit TimedAtomicValue(const T & val)
{ set(val); }
- T get()
+ TimedAtomicValue(const T & val, Timestamp timestamp)
+ { set(val, timestamp); }
+
+ explicit TimedAtomicValue(const TimedValue<T> val)
+ { set(val); }
+
+ void set(const T & val, Timestamp timestamp)
{
std::lock_guard <std::mutex> lock{mutex};
- return val;
+ this->val = val;
+ timestamp = timestamp;
}
void set(const T & val)
+ { set(val, networking::time::now()); }
+
+ void set(const TimedValue<T> & val)
+ { set(val.get(), val.getTimestamp()); }
+
+ TimedAtomicValue & operator=(const T & val)
+ { set(val); }
+
+ TimedAtomicValue & operator=(const TimedValue<T> & val)
+ { set(val); }
+
+ T get()
{
std::lock_guard <std::mutex> lock{mutex};
- this->val = val;
- timestamp = networking::time::now();
+ return val;
}
Timestamp getTimestamp()
@@ -85,9 +108,6 @@ public:
return timestamp;
}
- TimedAtomicValue & operator=(const T & val)
- { set(val); }
-
explicit operator T()
{ return get(); }
diff --git a/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt b/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
index 9afaefb4..f2b61086 100644
--- a/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/PC2CarLib/CMakeLists.txt
@@ -50,11 +50,6 @@ find_package(NetworkingLib REQUIRED)
target_include_directories(PC2CarLib PUBLIC ${NetworkingLib_INCLUDE_DIRS})
target_link_libraries(PC2CarLib NetworkingLib)
-# PlatoonProtocolLib
-find_package(PlatoonProtocolLib REQUIRED)
-target_include_directories(PC2CarLib PUBLIC ${PlatoonProtocolLib_INCLUDE_DIRS})
-target_link_libraries(PC2CarLib PlatoonProtocolLib)
-
find_package(Boost REQUIRED COMPONENTS system regex)
link_libraries(${Boost_LIBRARIES})
@@ -126,9 +121,5 @@ add_executable(PC2CarTest ${TEST_SOURCE_FILES})
target_include_directories(PC2CarTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
target_link_libraries(PC2CarTest NetworkingLib)
-# PlatoonProtocolLib
-target_include_directories(PC2CarTest PUBLIC ${PlatoonProtocolLib_INCLUDE_DIRS})
-target_link_libraries(PC2CarTest PlatoonProtocolLib)
-
# For debugging
target_compile_options(PC2CarTest PUBLIC -fopenmp -fPIC -O0 -g3 -ggdb)
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
index 6008663a..247fa399 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
@@ -37,7 +37,7 @@ public:
void stop() override;
- PlatoonConfig getPlatoonConfig() const noexcept;
+ networking::time::TimedValue<PlatoonConfig> getPlatoonConfig();
void setOnPlatoonConfigUpdatedCallback(const Callback & callback)
{ onPlatoonConfigUpdatedCallback = callback; }
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
index 6da9b3a8..ce5d6a1a 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
@@ -12,6 +12,7 @@
#include "NetworkingLib/ServiceServer.h"
#include "NetworkingLib/ServiceClient.h"
#include "NetworkingLib/Resolver.h"
+#include "NetworkingLib/TimedValue.h"
#include "NetworkInfo.h"
#include "PlatoonMessage.h"
@@ -76,7 +77,7 @@ protected:
const NetworkInfo myInfo;
PlatoonId platoonId;
- std::atomic<PlatoonConfig> platoonConfig{DEFAULT_PLATOON_CONFIG};
+ networking::time::TimedAtomicValue<PlatoonConfig> platoonConfig{DEFAULT_PLATOON_CONFIG};
State state{State::IDLE};
std::mutex stateMutex;
networking::Networking & net;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
index 816c297a..a3396c1e 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/VehicleFacade.h
@@ -50,7 +50,7 @@ public:
// FollowerVehicle methods
- PlatoonConfig getPlatoonConfig();
+ networking::time::TimedValue<PlatoonConfig> getPlatoonConfig();
private:
Vehicle::Ptr vehicle;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
index e30ac786..74adffec 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
@@ -35,9 +35,9 @@ void FollowerVehicle::stop()
stopBroadcastTimer();
}
-PlatoonConfig FollowerVehicle::getPlatoonConfig() const noexcept
+networking::time::TimedValue<PlatoonConfig> FollowerVehicle::getPlatoonConfig()
{
- return platoonConfig.load();
+ return platoonConfig.getNonAtomicCopy();
}
void FollowerVehicle::doCreatePlatoon()
@@ -165,7 +165,7 @@ void FollowerVehicle::receiveBroadcast(const PlatoonMessage & broadcast)
startBroadcastTimer();
PlatoonConfig newConfig{broadcast.platoonSpeed, broadcast.innerPlatoonDistance};
- if (platoonConfig.load() != newConfig)
+ if (platoonConfig.get() != newConfig)
{
platoonConfig = newConfig;
onPlatoonConfigUpdatedCallback();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
index a908df36..e485fb4b 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
@@ -40,12 +40,12 @@ void LeaderVehicle::setPlatoonConfig(const PlatoonConfig & platoonConfig)
void LeaderVehicle::setPlatoonSpeed(PlatoonSpeed platoonSpeed)
{
- platoonConfig = PlatoonConfig{platoonSpeed, platoonConfig.load().innerPlatoonDistance};
+ platoonConfig = PlatoonConfig{platoonSpeed, platoonConfig.get().innerPlatoonDistance};
}
void LeaderVehicle::setInnerPlatoonDistance(InnerPlatoonDistance innerPlatoonDistance)
{
- platoonConfig = PlatoonConfig{platoonConfig.load().platoonSpeed, innerPlatoonDistance};
+ platoonConfig = PlatoonConfig{platoonConfig.get().platoonSpeed, innerPlatoonDistance};
}
std::size_t LeaderVehicle::getNumFollowers()
@@ -215,7 +215,7 @@ void LeaderVehicle::stopBroadcasting()
void LeaderVehicle::sendBroadcastMessage()
{
- PlatoonConfig configCopy = platoonConfig.load();
+ PlatoonConfig configCopy = platoonConfig.get();
// Send the broadcast message and forget about it.
sendMessage(PlatoonMessage::broadcastMessage(
myInfo.vehicleId,
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
index db4ea64e..f04d83c7 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/VehicleFacade.cpp
@@ -111,7 +111,7 @@ void VehicleFacade::setInnerPlatoonDistance(InnerPlatoonDistance innerPlatoonDis
std::dynamic_pointer_cast<LeaderVehicle>(vehicle)->setInnerPlatoonDistance(innerPlatoonDistance);
}
-PlatoonConfig VehicleFacade::getPlatoonConfig()
+networking::time::TimedValue<PlatoonConfig> VehicleFacade::getPlatoonConfig()
{
std::lock_guard<std::mutex> lock(mutex);
return std::dynamic_pointer_cast<FollowerVehicle>(vehicle)->getPlatoonConfig();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
index 06c38c0f..d3b73d39 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
@@ -30,8 +30,8 @@ int main(int argc, char ** argv)
[follower]
{
auto config = follower->getPlatoonConfig();
- std::cout << "Config Update:\nIPD=" << config.innerPlatoonDistance
- << "\nPS=" << config.platoonSpeed << "\n";
+ std::cout << "Config Update:\nIPD=" << config.get().innerPlatoonDistance
+ << "\nPS=" << config.get().platoonSpeed << "\n";
});
follower->createPlatoon();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
index 9fe2c35c..763b2b15 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
@@ -78,7 +78,7 @@ bool test2Follower1()
return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
{
return follower1->isPlatoonRunning() &&
- follower1->getPlatoonConfig() == TEST_CONFIG;
+ follower1->getPlatoonConfig().get() == TEST_CONFIG;
});
}
@@ -126,7 +126,7 @@ bool test3Follower1()
auto deltaTime = now - lastConfigTime;
lastConfigTime = now;
- if (follower1->getPlatoonConfig() != expectedConfig)
+ if (follower1->getPlatoonConfig().get() != expectedConfig)
{
success = false;
running = false;
@@ -293,8 +293,8 @@ bool test8Followers(FollowerVehicle::Ptr follower)
{
auto delta = networking::time::now() - last;
std::cout << "time passed [ms]: " << std::chrono::duration_cast<std::chrono::milliseconds>(delta).count() << "\n";
- std::cout << "ps: " << follower->getPlatoonConfig().platoonSpeed << "\n";
- if (follower->getPlatoonConfig() != expectedConfig)
+ std::cout << "ps: " << follower->getPlatoonConfig().get().platoonSpeed << "\n";
+ if (follower->getPlatoonConfig().get() != expectedConfig)
{
success = false;
return;
diff --git a/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h b/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
index b176cfc1..62889e79 100644
--- a/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
+++ b/modules/catkin_ws/src/VeloxProtocolLib/include/VeloxProtocolLib/Connection.h
@@ -75,8 +75,6 @@ private:
networking::BusyLock busyLock;
Connection::Ptr self;
-
- void closeOnce();
};
networking::Networking & net;
diff --git a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
index 14beb787..41210518 100644
--- a/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
+++ b/modules/catkin_ws/src/car/include/mainNode/PlatoonController.h
@@ -33,17 +33,18 @@ public:
Callback cruiseControllerNotify;
- // TODO make values ATOMIC !
- // these value need to be stored and atomic, since they will be pulled from other modules
- PlatoonState curState = PlatoonState::ACC;
+ PlatoonState getCurrState() const
+ { return currState; }
- platoonProtocol::PlatoonConfig platoonConfig; // TODO needs to be removed, once C2C returns TimedValues
+ networking::time::TimedValue<platoonProtocol::PlatoonConfig> getPlatoonConfig()
+ { return platoonConfig.getNonAtomicCopy(); }
private:
platoonProtocol::VehicleFacade & c2c;
pc2car::CommandReceiver::Ptr pc;
EgoMotion & egoMotion;
-
+ std::atomic<PlatoonState> currState{PlatoonState::ACC};
+ networking::time::TimedAtomicValue<platoonProtocol::PlatoonConfig> platoonConfig;
bool c2cAlive = false;
// void updateC2cConfig(); // once C2C returns TimedValues
diff --git a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
index 24c3ec17..49dfdb02 100644
--- a/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/CruiseController.cpp
@@ -22,13 +22,13 @@ CruiseController::CruiseController(platoonProtocol::VehicleFacade& c2c,
}
void CruiseController::run() {
- switch (platoonController.curState) {
+ switch (platoonController.getCurrState()) {
case PlatoonState::ACC: {
run_ACC(platoonController.getDesSpeed());
break;
}
case PlatoonState::CACC_LV: {
- platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+ auto config = platoonController.getPlatoonConfig().get();
run_ACC(config.platoonSpeed);
c2c.setPlatoonSpeed(speed);
break;
@@ -57,15 +57,13 @@ void CruiseController::run_ACC(float target_speed) {
ccDataMsg outMsg;
outMsg.speed = speed;
ccData.publish(outMsg);
-
- return;
}
void CruiseController::run_CACC() {
std::cout << "CruiseController::run_CACC" << std::endl;
- platoonProtocol::PlatoonConfig config = platoonController.platoonConfig;
+ auto config = platoonController.getPlatoonConfig().get();
float PS = config.platoonSpeed;
float IPD = config.innerPlatoonDistance;
float vp = egoMotion.getPrvSpeed();
@@ -84,8 +82,6 @@ void CruiseController::run_CACC() {
ccDataMsg outMsg;
outMsg.speed = speed;
ccData.publish(outMsg);
-
- return;
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
index 5e48ea50..b2d853d7 100644
--- a/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/PlatoonController.cpp
@@ -68,7 +68,7 @@ void PlatoonController::setupC2C()
void PlatoonController::run()
{
std::cout << "PlatoonController was run." << std::endl;
- switch (curState) {
+ switch (currState.load()) {
case PlatoonState::ACC: { run_ACC(); break; }
case PlatoonState::CACC_FV: { run_CACC_FV(); break; }
case PlatoonState::CACC_LV: { run_CACC_LV(); break; }
@@ -96,9 +96,9 @@ void PlatoonController::run_ACC()
if (c2c.isPlatoonRunning()) {
if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {
updatePcConfig();
- curState = PlatoonState::CACC_LV;
+ currState = PlatoonState::CACC_LV;
}
- else {curState = PlatoonState::CACC_FV;}
+ else {currState = PlatoonState::CACC_FV;}
return;
}
@@ -116,7 +116,6 @@ void PlatoonController::run_ACC()
// role fits + !inPlatoon + c2cAlive + wantsPlatoon
if (c2c.getRole() == platoonProtocol::Vehicle::Role::LEADER) {updatePcConfig();}
updateDesSpeed();
- return;
}
void PlatoonController::run_CACC_FV()
@@ -136,10 +135,9 @@ void PlatoonController::run_CACC_FV()
if (inPlatoon && !wantsPlatoon) { c2c.leavePlatoon(); }
c2cAlive = false;
- curState = PlatoonState::ACC;
+ currState = PlatoonState::ACC;
cruiseControllerNotify();
run_ACC();
- return;
}
void PlatoonController::run_CACC_LV()
@@ -161,10 +159,9 @@ void PlatoonController::run_CACC_LV()
}
c2cAlive = false;
- curState = PlatoonState::ACC;
+ currState = PlatoonState::ACC;
cruiseControllerNotify();
run_ACC();
- return;
}
}
--
GitLab