-

modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h

@@ -59,7 +59,6 @@ private:
     VehicleId leader;
     std::vector<VehicleId> vehiclesToRequest;
     std::vector<VehicleId>::iterator vehiclesToRequestIter;
-    bool waitingForResponse{false};
     networking::time::Timer::Ptr responseTimer;
     networking::time::Timer::Ptr broadcastTimer;
     networking::time::Timer::Ptr heartbeatTimer;

modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp

@@ -59,8 +59,6 @@ void FollowerVehicle::doRunPlatoon()
     if (state != State::CREATING_PLATOON)
         return;
 
-    log("Running Platoon!\n");
-
     Vehicle::doRunPlatoon();
 
     startHeartbeat();
@@ -73,8 +71,6 @@ void FollowerVehicle::doLeavePlatoon()
     if (state == State::IDLE)
         return;
 
-    log("Leave Platoon!\n");
-
     Vehicle::doLeavePlatoon();
 
     sendLeavePlatoonMessage();
@@ -105,29 +101,24 @@ void FollowerVehicle::receiveMessage(const PlatoonMessage & message)
 
 void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle()
 {
-    if (state != State::CREATING_PLATOON)
+    if (state != State::CREATING_PLATOON || vehiclesToRequest.empty())
         return;
 
-    if (!waitingForResponse &&
-        vehiclesToRequest.size() > 0)
-    {
-        utils::cycle(vehiclesToRequestIter, vehiclesToRequest);
-
-        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(
-            RESPONSE_TIMEOUT,
-            [self]
-            {
-                log("Response Timeout!\n");
-                self->sendPlatoonCreateRequestToNextVehicle();
-            });
-    }
+    utils::cycle(vehiclesToRequestIter, vehiclesToRequest);
+
+    auto vehicleToRequest = *vehiclesToRequestIter;
+    sendPlatoonCreateRequest(vehicleToRequest);
+
+    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(
+        RESPONSE_TIMEOUT,
+        [self]
+        {
+            log("Response Timeout!\n");
+            self->sendPlatoonCreateRequestToNextVehicle();
+        });
 }
 
 void FollowerVehicle::sendPlatoonCreateRequest(VehicleId vehicleToRequest)
@@ -204,12 +195,10 @@ void FollowerVehicle::receiveResponse(const PlatoonMessage & response)
 {
     if (state != State::CREATING_PLATOON ||
         response.dstVehicle != myInfo.vehicleId ||
-        !waitingForResponse ||
         response.srcVehicle != *vehiclesToRequestIter)
         return;
 
     responseTimer->stop();
-    waitingForResponse = false;
 
     if (response.messageType == messageTypes::LV_ACCEPT)
     {