diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h index 41c20b7788b1e2cc2ce015362a8e6819457c9031..d38f8f4a3a5e68efa3567761d67982c54707f4f1 100644 --- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h +++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h @@ -52,6 +52,7 @@ protected: Ptr shared_from_this(); private: + static constexpr networking::time::Duration RESPONSE_TIMEOUT = std::chrono::milliseconds(3000); static constexpr networking::time::Duration HEARTBEAT_INTERVAL = std::chrono::milliseconds(50); static constexpr networking::time::Duration BROADCAST_TIMEOUT = std::chrono::milliseconds(500); diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp index 9b8e6525969b10166a5ba5a52f49b11abdc366af..f7cc4814afe8d50a3218fe4c86e0dcb5e705141a 100644 --- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp +++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp @@ -13,6 +13,7 @@ using namespace std::chrono_literals; constexpr networking::time::Duration FollowerVehicle::HEARTBEAT_INTERVAL; constexpr networking::time::Duration FollowerVehicle::BROADCAST_TIMEOUT; +constexpr networking::time::Duration FollowerVehicle::RESPONSE_TIMEOUT; FollowerVehicle::Ptr FollowerVehicle::create(networking::Networking & net, const NetworkInfo & info) { @@ -116,9 +117,12 @@ void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle() // Create a new one to make sure we're never getting a busy exception. responseTimer = networking::time::Timer::create(net); responseTimer->startTimeout( - 3s, + RESPONSE_TIMEOUT, [self] - { self->sendPlatoonCreateRequestToNextVehicle(); }); + { + log("Response Timeout!\n"); + self->sendPlatoonCreateRequestToNextVehicle(); + }); utils::cycle(vehiclesToRequestIter, vehiclesToRequest); } @@ -208,6 +212,8 @@ void FollowerVehicle::receiveResponse(const PlatoonMessage & response) response.srcVehicle != *vehiclesToRequestIter) return; + log("Received response!\n"); + responseTimer->stop(); waitingForResponse = false;