From 910d4b063f698ea2079bea37d1a6c2d0a7b2c767 Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Wed, 4 Apr 2018 00:08:27 +0200
Subject: [PATCH] -
---
.../include/PlatoonProtocolLib/Utils.h | 2 +-
.../PlatoonProtocolLib/src/FollowerVehicle.cpp | 15 +++++++++++----
2 files changed, 12 insertions(+), 5 deletions(-)
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
index a5a208a8..91b78697 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
@@ -25,7 +25,7 @@ namespace utils
template<typename ForwardIterator, typename Container>
void cycle(ForwardIterator & iter, Container & container)
{
- if (std::next(iter) == container.end())
+ if (iter == container.end() || std::next(iter) == container.end())
iter = container.begin();
else
iter++;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
index f7cc4814..96231746 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
@@ -48,7 +48,9 @@ void FollowerVehicle::doCreatePlatoon()
Vehicle::doCreatePlatoon();
// We cycle through every available endpoint and try to make a create-platoon-request.
- vehiclesToRequestIter = vehiclesToRequest.begin();
+ // Note that we'll be actually starting with the first element in the vector
+ // by setting the iterator to end() due to utils::cycle().
+ vehiclesToRequestIter = vehiclesToRequest.end();
sendPlatoonCreateRequestToNextVehicle();
}
@@ -107,8 +109,10 @@ void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle()
return;
if (!waitingForResponse &&
- vehiclesToRequestIter != vehiclesToRequest.end()) // in case of an empty vector of vehicle endpoints
+ vehiclesToRequest.size() > 0)
{
+ utils::cycle(vehiclesToRequestIter, vehiclesToRequest);
+
auto vehicleToRequest = *vehiclesToRequestIter;
sendPlatoonCreateRequest(vehicleToRequest);
@@ -123,8 +127,6 @@ void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle()
log("Response Timeout!\n");
self->sendPlatoonCreateRequestToNextVehicle();
});
-
- utils::cycle(vehiclesToRequestIter, vehiclesToRequest);
}
}
@@ -206,6 +208,11 @@ void FollowerVehicle::receiveBroadcast(const PlatoonMessage & broadcast)
void FollowerVehicle::receiveResponse(const PlatoonMessage & response)
{
+ log("Response received:\n",
+ "state: ", state, "\n",
+ "response.dstVehicle: ", response.dstVehicle, "\n",
+ "waitingForResponse: ", waitingForResponse, "\n")
+
if (state != State::CREATING_PLATOON ||
response.dstVehicle != myInfo.vehicleId ||
!waitingForResponse ||
--
GitLab