From 4265974a03f2853404e1c7cdb565d854fce3759e Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Mon, 2 Apr 2018 16:56:15 +0200
Subject: [PATCH] it builds
---
.../PlatoonProtocolLib/LeaderVehicle.h | 6 +-
.../PlatoonProtocolLib/PlatoonMessage.h | 2 +-
.../include/PlatoonProtocolLib/Utils.h | 6 +-
.../include/PlatoonProtocolLib/Vehicle.h | 9 +--
.../src/FollowerVehicle.cpp | 12 ++--
.../PlatoonProtocolLib/src/LeaderVehicle.cpp | 64 +++++++++----------
.../src/PlatoonProtocolLib/src/Vehicle.cpp | 25 ++++++--
.../PlatoonProtocolLib/test/ProtocolTest.cpp | 22 +++----
8 files changed, 82 insertions(+), 64 deletions(-)
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
index b9c888ac..c45b828f 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
@@ -28,7 +28,7 @@ private:
public:
using Ptr = std::shared_ptr<LeaderVehicle>;
- static Ptr create(networking::Networking & net, VehicleId ownVehicleId);
+ static Ptr create(networking::Networking & net, const NetworkInfo & info);
// This should not be used outside.
LeaderVehicle(PrivateTag, networking::Networking & net, const NetworkInfo & info);
@@ -66,7 +66,7 @@ private:
void receivePlatoonCreateRequest(const PlatoonMessage & request);
- bool findFollower(VehicleId id);
+ bool findFollower(VehicleId id) const;
void addFollower(VehicleId follower);
@@ -85,6 +85,8 @@ private:
void stopBroadcasting();
void removeAllFollowers();
+
+ bool checkFollowerMessage(const PlatoonMessage & message) const;
};
}
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h
index 267795fb..6fe9270a 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h
@@ -192,7 +192,7 @@ struct Decoder<platoonProtocol::PlatoonMessage>
{
message.platoonSpeed = j.at(PLATOON_SPEED).get<PlatoonSpeed>();
message.innerPlatoonDistance = j.at(INNER_PLATOON_DISTANCE).get<InnerPlatoonDistance>();
- message.followers = j.at(FOLLOWERS).get<PlatoonMessage::Followers>;
+ message.followers = j.at(FOLLOWERS).get<PlatoonMessage::Followers>();
}
};
};
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
index e83ae0f3..a5a208a8 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Utils.h
@@ -37,10 +37,10 @@ void cycle(ForwardIterator & iter, Container & container)
* @param map
* @return A vector of the keys contained in map
*/
-template<template<class> class Map, typename T>
-std::vector<T> keys(const Map<T> & map)
+template<template<typename...> class Map, typename Key, typename Value>
+std::vector<Key> keys(const Map<Key, Value> & map)
{
- std::vector<T> mapKeys;
+ std::vector<Key> mapKeys;
for (const auto & i : map)
mapKeys.emplace_back(i.first);
return mapKeys;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
index 49dd3e73..6bcdfed9 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
@@ -82,11 +82,11 @@ protected:
Callback onRunningPlatoonCallback;
Callback onLeavingPlatoonCallback;
- virtual void doCreatePlatoon() = 0;
+ virtual void doCreatePlatoon();
- virtual void doRunPlatoon() = 0;
+ virtual void doRunPlatoon();
- virtual void doLeavePlatoon() = 0;
+ virtual void doLeavePlatoon();
virtual void receiveMessage(const PlatoonMessage & message) = 0;
@@ -94,10 +94,11 @@ protected:
void setState(State newState);
+ void receive();
+
private:
Role role;
- void receive();
};
}
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
index 9da71dc8..912fbe4c 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
@@ -44,6 +44,8 @@ void FollowerVehicle::doCreatePlatoon()
setState(State::CREATING_PLATOON);
+ receive();
+
// We cycle through every available endpoint and try to make a create-platoon-request.
vehiclesToRequestIter = vehiclesToRequest.begin();
sendPlatoonCreateRequestToNextVehicle();
@@ -72,8 +74,6 @@ void FollowerVehicle::doLeavePlatoon()
sendLeavePlatoonMessage();
- setState(State::IDLE);
-
onLeavingPlatoonCallback();
}
@@ -111,7 +111,7 @@ void FollowerVehicle::sendPlatoonCreateRequestToNextVehicle()
waitingForResponse = true;
auto vehicleToRequest = *vehiclesToRequestIter;
sendPlatoonCreateRequest(vehicleToRequest);
- utils::cycle(vehiclesToRequestIter, vehicleToRequest);
+ utils::cycle(vehiclesToRequestIter, vehiclesToRequest);
}
auto self = shared_from_this();
@@ -139,8 +139,8 @@ void FollowerVehicle::startHeartbeat()
HEARTBEAT_INTERVAL,
[self]
{
- sendMessage(PlatoonMessage::heartbeatMessage(self->myInfo.vehicleId, leader, self->platoonId),
- HEARTBEAT_INTERVAL);
+ self->sendMessage(PlatoonMessage::heartbeatMessage(self->myInfo.vehicleId, self->leader, self->platoonId),
+ HEARTBEAT_INTERVAL);
});
}
@@ -210,7 +210,7 @@ void FollowerVehicle::addVehicleToRequest(VehicleId vehicleId)
auto self = shared_from_this();
net.callLater(
[self, vehicleId]
- { vehiclesToRequest.push_back(vehicleId); });
+ { self->vehiclesToRequest.push_back(vehicleId); });
}
FollowerVehicle::Ptr FollowerVehicle::shared_from_this()
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
index b73b8248..893c7859 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
@@ -13,9 +13,9 @@ using namespace std::chrono_literals;
constexpr networking::time::Duration LeaderVehicle::HEARTBEAT_TIMEOUT;
constexpr networking::time::Duration LeaderVehicle::BROADCAST_INTERVAL;
-LeaderVehicle::Ptr LeaderVehicle::create(networking::Networking & net, VehicleId ownVehicleId)
+LeaderVehicle::Ptr LeaderVehicle::create(networking::Networking & net, const NetworkInfo & info)
{
- return std::make_shared<LeaderVehicle>(PrivateTag{}, net, ownVehicleId);
+ return std::make_shared<LeaderVehicle>(PrivateTag{}, net, info);
}
LeaderVehicle::LeaderVehicle(PrivateTag, networking::Networking & net, const NetworkInfo & info)
@@ -49,7 +49,7 @@ void LeaderVehicle::doCreatePlatoon()
if (state != State::IDLE)
return;
- setState(State::CREATING_PLATOON);
+ Vehicle::doCreatePlatoon();
// Generate a platoon id.
platoonId = rand();
@@ -60,7 +60,7 @@ void LeaderVehicle::doRunPlatoon()
if (state != State::CREATING_PLATOON)
return;
- setState(State::RUNNING_PLATOON);
+ Vehicle::doRunPlatoon();
startBroadcasting();
@@ -72,9 +72,7 @@ void LeaderVehicle::doLeavePlatoon()
if (state == State::IDLE)
return;
- stop();
-
- setState(State::IDLE);
+ Vehicle::doLeavePlatoon();
onLeavingPlatoonCallback();
}
@@ -110,7 +108,7 @@ void LeaderVehicle::receivePlatoonCreateRequest(const PlatoonMessage & request)
doRunPlatoon();
}
-bool LeaderVehicle::findFollower(VehicleId id)
+bool LeaderVehicle::findFollower(VehicleId id) const
{
return followers.find(id) != followers.end();
}
@@ -166,22 +164,24 @@ void LeaderVehicle::stopHeartbeatTimer(LeaderVehicle::Follower & follower)
void LeaderVehicle::receiveHeartbeat(const PlatoonMessage & heartbeat)
{
- if (state != State::RUNNING_PLATOON)
+ if (!checkFollowerMessage(heartbeat))
return;
- auto followerId = heartbeat.srcVehicle;
-
- if (findFollower(followerId) && heartbeat.platoonId == platoonId)
- {
- auto & follower = followers[followerId];
- stopHeartbeatTimer(follower);
- startHeartbeatTimer(follower);
- };
+ auto & follower = followers[heartbeat.srcVehicle];
+ stopHeartbeatTimer(follower);
+ startHeartbeatTimer(follower);
}
void LeaderVehicle::receiveLeavePlatoonMessage(const PlatoonMessage & message)
{
+ if (!checkFollowerMessage(message))
+ return;
+ auto & follower = followers[message.srcVehicle];
+ removeFollower(follower);
+ // If there are no more followers, we leave the platoon too.
+ if (followers.empty())
+ doLeavePlatoon();
}
void LeaderVehicle::startBroadcasting()
@@ -196,22 +196,13 @@ void LeaderVehicle::startBroadcasting()
PlatoonConfig configCopy = self->platoonConfig.load();
- for (auto & pair : self->followers)
- {
- auto & follower = pair.second;
-
- auto broadcastMsg = PlatoonMessage::broadcastMessage(
- self->getOwnEndpoint().getVehicleId(),
- self->platoonId,
- configCopy.platoonSpeed,
- configCopy.innerPlatoonDistance);
-
- // Send the broadcast message and forget about it.
- follower.broadcastSender->asyncSend(
- broadcastMsg, follower.endpoint.getHost(), follower.endpoint.getUdpPort(), BROADCAST_INTERVAL,
- [self](auto && ...)
- {});
- }
+ // Send the broadcast message and forget about it.
+ self->sendMessage(PlatoonMessage::broadcastMessage(
+ self->myInfo.vehicleId,
+ self->platoonId,
+ configCopy.platoonSpeed,
+ configCopy.innerPlatoonDistance,
+ utils::keys(self->followers)), BROADCAST_INTERVAL);
});
}
@@ -227,6 +218,13 @@ void LeaderVehicle::removeAllFollowers()
{ this->removeFollower(follower); });
}
+bool LeaderVehicle::checkFollowerMessage(const PlatoonMessage & message) const
+{
+ return state == State::RUNNING_PLATOON &&
+ !findFollower(message.srcVehicle) &&
+ message.platoonId != platoonId;
+}
+
LeaderVehicle::Ptr LeaderVehicle::shared_from_this()
{
return std::static_pointer_cast<LeaderVehicle>(Vehicle::shared_from_this());
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
index 09f19c67..2a3441a5 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
@@ -8,7 +8,8 @@
#include "../include/PlatoonProtocolLib/Vehicle.h"
#include "../include/PlatoonProtocolLib/Utils.h"
-using namespace platoonProtocol;
+namespace platoonProtocol
+{
constexpr std::uint16_t Vehicle::UDP_PORT;
constexpr PlatoonConfig Vehicle::DEFAULT_PLATOON_CONFIG;
@@ -22,8 +23,6 @@ Vehicle::Vehicle(networking::Networking & net,
{
receiver = networking::message::DatagramReceiver<PlatoonMessage>::create(net, UDP_PORT, PlatoonMessage::MAX_SIZE);
sender = networking::message::DatagramSender<PlatoonMessage>::create(net);
-
- receive();
}
bool Vehicle::isIdle()
@@ -58,6 +57,24 @@ void Vehicle::stop()
{
receiver->stop();
sender->stop();
+
+ setState(State::IDLE);
+}
+
+void Vehicle::doCreatePlatoon()
+{
+ setState(State::CREATING_PLATOON);
+ receive();
+}
+
+void Vehicle::doRunPlatoon()
+{
+ setState(State::RUNNING_PLATOON);
+}
+
+void Vehicle::doLeavePlatoon()
+{
+ stop();
}
void Vehicle::setState(Vehicle::State newState)
@@ -73,7 +90,7 @@ void Vehicle::receive()
networking::time::Duration::max(),
[self](const auto & error, auto & message, const auto & host, auto port)
{
- if (state != State::IDLE && !error && message.dstVehicle == myInfo.vehicleId)
+ if (self->state != State::IDLE && !error && message.dstVehicle == self->myInfo.vehicleId)
self->receiveMessage(message);
self->receive();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/ProtocolTest.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/ProtocolTest.cpp
index c199676c..1ae75ede 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/test/ProtocolTest.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/ProtocolTest.cpp
@@ -9,6 +9,8 @@
using namespace platoonProtocol;
+const std::string broadcastAddress = "127.0.0.1";
+
void test::testBasicSetup()
{
networking::Networking net1;
@@ -16,16 +18,14 @@ void test::testBasicSetup()
networking::Networking net3;
using namespace platoonProtocol;
- auto leader = LeaderVehicle::create(net1, 1);
- auto follower1 = FollowerVehicle::create(net2, 2);
- auto follower2 = FollowerVehicle::create(net3, 3);
+ auto leader = LeaderVehicle::create(net1, NetworkInfo{1, broadcastAddress});
+ auto follower1 = FollowerVehicle::create(net2, NetworkInfo{2, broadcastAddress});
+ auto follower2 = FollowerVehicle::create(net3, NetworkInfo{3, broadcastAddress});
- leader->addVehicleEndpoint(2, "127.0.0.1");
- leader->addVehicleEndpoint(3, "127.0.0.1");
- follower1->addVehicleEndpoint(1, "127.0.0.1");
- follower1->addVehicleEndpoint(3, "127.0.0.1");
- follower2->addVehicleEndpoint(1, "127.0.0.1");
- follower2->addVehicleEndpoint(2, "127.0.0.1");
+ follower1->addVehicleToRequest(1);
+ follower1->addVehicleToRequest(3);
+ follower2->addVehicleToRequest(1);
+ follower2->addVehicleToRequest(2);
leader->setPlatoonConfig(PlatoonConfig{10.0f, 20.0f});
@@ -99,14 +99,14 @@ void test::testStop()
// Test if the destruction works as expected.
{
- auto leader = LeaderVehicle::create(net, 1);
+ auto leader = LeaderVehicle::create(net, NetworkInfo{1, broadcastAddress});
leader->createPlatoon();
sleep(1);
}
std::cout << "LEADER SUCCESS!\n";
sleep(1);
{
- auto follower = FollowerVehicle::create(net, 1);
+ auto follower = FollowerVehicle::create(net, NetworkInfo{1, broadcastAddress});
follower->createPlatoon();
sleep(1);
}
--
GitLab