diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h index cde9cc657b89b3233f5e2da6d3739637924dc5b4..d82192fd01fecad323145cc4eb855783a8ce4b44 100644 --- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h +++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h @@ -59,6 +59,7 @@ private: std::vector<VehicleId> vehiclesToRequest; std::vector<VehicleId>::iterator vehiclesToRequestIter; bool waitingForResponse{false}; + networking::time::Timer::Ptr responseTimer; networking::time::TimePoint lastBroadcastTimestamp; networking::time::Timer::Ptr heartbeatTimer; Callback onPlatoonConfigUpdatedCallback; diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp index 9757aa3609c1de46a59f66a25cfceed42f6aaa07..7fe6b86b52d03d18f0e0e1db678045979e51efce 100644 --- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp +++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp @@ -102,22 +102,28 @@ void FollowerVehicle::receiveMessage(const PlatoonMessage & message) void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle() { + log("sendPlatoonCreateRequestToNextVehicle"); + if (state != State::CREATING_PLATOON) return; if (!waitingForResponse && vehiclesToRequestIter != vehiclesToRequest.end()) // in case of an empty vector of vehicle endpoints { - waitingForResponse = true; auto vehicleToRequest = *vehiclesToRequestIter; sendPlatoonCreateRequest(vehicleToRequest); + + waitingForResponse = true; + auto self = shared_from_this(); + // Create a new one to make sure we're never getting a busy exception. + responseTimer = networking::time::Timer::create(net); + responseTimer->startTimeout( + 3s, + [self] + { self->sendPlatoonCreateRequestToNextVehicle(); }); + utils::cycle(vehiclesToRequestIter, vehiclesToRequest); } - - auto self = shared_from_this(); - net.callLater( - [self] - { self->sendPlatoonCreateRequestToNextVehicle(); }); } void FollowerVehicle::sendPlatoonCreateRequest(VehicleId vehicleToRequest) @@ -184,19 +190,26 @@ void FollowerVehicle::receiveBroadcast(const PlatoonMessage & broadcast) void FollowerVehicle::receiveResponse(const PlatoonMessage & response) { + log("Receiving response"); + if (state != State::CREATING_PLATOON || !waitingForResponse || response.srcVehicle != *vehiclesToRequestIter) return; + responseTimer->stop(); + waitingForResponse = false; + if (response.messageType == messageTypes::LV_ACCEPT) { leader = response.srcVehicle; platoonId = response.platoonId; doRunPlatoon(); } - - waitingForResponse = false; + else + { + sendPlatoonCreateRequestToNextVehicle(); + } } void FollowerVehicle::sendLeavePlatoonMessage() diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp index 893c7859fc1f67123197d627483dcd933c00f7a8..ed59bd75a3d1684472285a3fda2bee8f7c1fd5c2 100644 --- a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp +++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp @@ -120,7 +120,6 @@ void LeaderVehicle::addFollower(VehicleId vehicleId) Follower follower; follower.vehicleId = vehicleId; - follower.heartbeatTimer = networking::time::Timer::create(net); { std::lock_guard<std::mutex> lock(followersMutex); @@ -142,6 +141,8 @@ void LeaderVehicle::startHeartbeatTimer(Follower & follower) { auto followerId = follower.vehicleId; auto self = shared_from_this(); + // Create a new one to make sure we're never getting a busy exception. + follower.heartbeatTimer = networking::time::Timer::create(net); follower.heartbeatTimer->startTimeout( HEARTBEAT_TIMEOUT, [self, followerId]