diff --git a/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h b/modules/catkin_ws/src/NetworkingLib/include/NetworkingLib/TimedValue.h index 10cbe19b50a0c94b744cd435b17cd7507436c95b..2107a0b2d9968290f78e86f8d7b3c2e4566102b5 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 9afaefb45060f2d3351f73f709b0758a75b6a768..f2b610864228e4bb2d07b7615ca153e7ae985faf 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 6008663a41564198133a8d7b8e46a286c497ded2..247fa3993545102284d0ff7e8e7234021fd8b529 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 6da9b3a89a2a7b3bf057be77571d3333be8d2667..ce5d6a1ad9357c325ae5d23afdb988e9dc9fa034 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 816c297a413c54ebed574b4bac9af14be095161f..a3396c1ec108bc71f8e915c3e5f1b52ea8cdacb2 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 e30ac786fa63d53429d888b366b0d3e47078e949..74adffec4971a8b31646d82f36f9e57b80189687 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 a908df36624545a834fa07b938efbd1cf556e45a..e485fb4bd1551ec0786cc88438f4e7f4343b2760 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 db4ea64ec269ec7f5181514153d740aefa12f498..f04d83c7751bc519f6fc3256796f8f4604f6c4fb 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 06c38c0f690ff31ecdfeb60ad563532cad391db7..d3b73d395cf604c9951c65bf4f20e66ff66c3a66 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 9fe2c35c785161d2cf85ed60ed09578d31e5cfc9..763b2b1539876b53edfbb4951f6c247bcb510367 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 b176cfc1aa260cd0128550a457f88f0c875c5ab6..62889e79f72eb52712217f67ab5ad8c39898fff2 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 14beb787d5abfae1fb9cbddc1b4736015f605605..412105186db157694ba275c02bfb785e2f670845 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 24c3ec17dcc66c2e6cf7f77f24b422c43c1c76d6..49dfdb02ca60427d46a9ded875067993b2c17a4a 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 5e48ea50f4822850bd695e51b5d95557f7cc2730..b2d853d7852d7945449894b1d93e198d75f25c00 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; } }