diff --git a/Scripts/TestMaster/TestMaster.py b/Scripts/TestMaster/TestMaster.py
new file mode 100644
index 0000000000000000000000000000000000000000..6a280fbc32e1c4432e3e19359b7c0677ee63b0a6
--- /dev/null
+++ b/Scripts/TestMaster/TestMaster.py
@@ -0,0 +1,122 @@
+from pexpect import pxssh
+
+
+class Node:
+ def __init__(self, username, hostname, password, testCommand):
+ self.username = username
+ self.hostname = hostname
+ self.password = password
+ self.testCommand = testCommand
+
+
+class NodeConnection(pxssh.pxssh):
+ def __init__(self, node):
+ pxssh.pxssh.__init__(self)
+ self.node = node
+ self.connected = False
+
+ def __enter__(self):
+ self.connect()
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.disconnect()
+
+ def connect(self):
+ """
+ Connects to a node via ssh. This may take a few seconds (due to the underlying module).
+ :return:
+ """
+ try:
+ self.login(self.node.hostname, self.node.username, self.node.password,
+ original_prompt="/home/" + self.node.username + ">")
+ except pxssh.ExceptionPxssh:
+ print("ERROR: failed to connect to node: " + self.node.ssh_id)
+ raise
+ self.connected = True
+ print("Connected to: " + str(self.node.username) + "@" + str(self.node.hostname))
+
+ def disconnect(self):
+ if self.connected:
+ self.logout()
+ self.connected = False
+ print("Disconnected from: " + str(self.node.username) + "@" + str(self.node.hostname))
+
+ def prepare_dump_settings(self, sim_id):
+ self.start_clt()
+ # set sim id
+ self.sendline("tx id " + str(sim_id))
+ # set number of coefficients to dump
+ self.sendline("rx ncoff 30")
+ self.stop_clt()
+
+ def start_transmissions(self, num_transmissions, transmission_delay):
+ """
+ Non-Blocking, initiates a broadcast transmission on the node.
+ :param num_transmissions: number of packets to broadcast
+ :param transmission_delay: time delay between each consecutive transmission in seconds
+ which must be in the range from 0.05 to 60
+ """
+ self.start_clt()
+ # the delay parameter is given in milliseconds
+ self.sendline("tx delay " + str(int(transmission_delay * 1000)))
+ self.clt_prompt()
+ # this is non blocking
+ self.sendline("tx " + str(num_transmissions))
+ self.clt_prompt()
+ self.stop_clt()
+
+ def get_dump(self, sender, sim_id):
+ rx_content = self.exec_cmd("cd /data/rx && ls -1")
+ dump_files = list(
+ filter(lambda filename: self.check_filename(filename, sender, sim_id), rx_content.split("\r\n")))
+ if len(dump_files) == 0:
+ raise FileNotFoundError("ERROR: Dump not found! (receiver: " + str(self.node.node_id) +
+ ", sender: " + str(sender.node_id) + ", sim_id: " + str(sim_id))
+ if len(dump_files) > 1:
+ raise Exception("ERROR: There are multiple dump files for receiver: " + str(self.node.node_id) +
+ ", sender: " + str(sender.node_id) + ", sim_id: " + str(sim_id) + "!")
+ content = self.exec_cmd("cat " + dump_files[0])
+ return Dump(self.node, sender, content)
+
+ def clean_dump_dir(self):
+ self.sendline("cd /data/rx && rm *")
+ self.prompt()
+
+ def start_clt(self):
+ self.sendline("clt")
+ self.clt_prompt()
+
+ def stop_clt(self):
+ self.sendline("quit")
+ self.prompt()
+
+ def clt_prompt(self):
+ self.expect("CMD>")
+
+ def check_filename(self, filename, sender, sim_id):
+ # Dumped receive data is written to a file with name "dump-RR-CCC-TT-DDD.txt" where RR
+ # identifies the receiver node, CCC is a run counter which increments when the daemon starts,
+ # TT identifies the transmitting node, and DDD is a test identifier (sim_id)
+ filename = filename.split(".txt")[0]
+ split = filename.split("-")
+ if len(split) < 5:
+ return False
+ fn_dump, fn_receiver, fn_run, fn_sender, fn_id = split
+ return fn_dump == "dump" \
+ and int(fn_receiver) == self.node.node_id \
+ and int(fn_sender) == sender.node_id \
+ and int(fn_id) == sim_id
+
+ def exec_cmd(self, cmd):
+ """
+ :param cmd: command to execute on the shell
+ :return: the output produced by the command as a string
+ """
+ self.sendline(cmd)
+ self.prompt()
+ return self.before.decode("utf-8").split(cmd + "\r\r\n")[1]
+
+
+if __name__ == "__main__":
+ print("hello")
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
index 7bc0a6137ef39742b88c4e9039f70cba84573486..d557cbd250b2055947670340480fca310848a56b 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
@@ -157,5 +157,9 @@ add_executable(FollowerTest ${SOURCE_FILES} test/FollowerTest.cpp)
target_link_libraries(FollowerTest NetworkingLib)
target_include_directories(FollowerTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
+add_executable(TestScenarios ${SOURCE_FILES} test/TestScenarios.cpp)
+target_link_libraries(TestScenarios NetworkingLib)
+target_include_directories(TestScenarios PUBLIC ${NetworkingLib_INCLUDE_DIRS})
+
# For debugging
#target_compile_options(LeaderTest PUBLIC -fopenmp -fPIC -O0 -g3 -ggdb)
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
index c1067561ec1cef192e4f91c7f71a6e24f2f220ca..6008663a41564198133a8d7b8e46a286c497ded2 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/FollowerVehicle.h
@@ -24,6 +24,10 @@ private:
};
public:
+ static constexpr networking::time::Duration RESPONSE_TIMEOUT = std::chrono::milliseconds(3000);
+ static constexpr networking::time::Duration HEARTBEAT_INTERVAL = std::chrono::milliseconds(200);
+ static constexpr networking::time::Duration BROADCAST_TIMEOUT = std::chrono::milliseconds(500);
+
using Ptr = std::shared_ptr<FollowerVehicle>;
static Ptr create(networking::Networking & net, const NetworkInfo & info);
@@ -50,15 +54,11 @@ 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(200);
- static constexpr networking::time::Duration BROADCAST_TIMEOUT = std::chrono::milliseconds(500);
-
VehicleId leader;
networking::time::Timer::Ptr responseTimer;
networking::time::Timer::Ptr broadcastTimer;
networking::time::Timer::Ptr heartbeatTimer;
- Callback onPlatoonConfigUpdatedCallback;
+ Callback onPlatoonConfigUpdatedCallback{DEFAULT_CALLBACK};
void sendPlatoonCreateRequest();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
index 5e8a491674a703dd3c3470818113e18d5ad2a808..58ea5b10ac89354801c3c46189873e4fef918d89 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/LeaderVehicle.h
@@ -26,6 +26,9 @@ private:
};
public:
+ static constexpr networking::time::Duration HEARTBEAT_TIMEOUT = std::chrono::milliseconds(1000);
+ static constexpr networking::time::Duration BROADCAST_INTERVAL = std::chrono::milliseconds(50);
+
using Ptr = std::shared_ptr<LeaderVehicle>;
static Ptr create(networking::Networking & net, const NetworkInfo & info);
@@ -55,9 +58,6 @@ protected:
Ptr shared_from_this();
private:
- static constexpr networking::time::Duration HEARTBEAT_TIMEOUT = std::chrono::milliseconds(1000);
- static constexpr networking::time::Duration BROADCAST_INTERVAL = std::chrono::milliseconds(50);
-
struct Follower
{
VehicleId vehicleId;
@@ -88,6 +88,8 @@ private:
void stopBroadcasting();
+ void sendBroadcastMessage();
+
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 cbcd60a9b6a84e8f96885debed0d68eff3b263c4..dff590acd3be3fd817ea6afc10ad64e9a6300320 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/PlatoonMessage.h
@@ -33,6 +33,13 @@ inline bool isStandardMessageType(MessageType messageType)
return messageTypes::standardMessageTypes.find(messageType) != messageTypes::standardMessageTypes.end();
}
+template<typename ... MessageTypes>
+bool contains(MessageType type, MessageTypes ... types)
+{
+ std::unordered_set<MessageType> set{types...};
+ return set.find(type) != set.end();
+}
+
}
namespace jsonNames
@@ -145,11 +152,16 @@ struct Encoder<platoonProtocol::PlatoonMessage>
using json = nlohmann::json;
using namespace jsonNames;
- auto j = json{{SRC_VEHICLE, message.srcVehicle},
- {DST_VEHICLE, message.dstVehicle},
- {PLATOON_ID, message.platoonId}};
+ auto j = json{{SRC_VEHICLE, message.srcVehicle}};
+
+ using namespace messageTypes;
+ if (messageTypes::contains(messageType, FV_HEARTBEAT, LV_ACCEPT, REJECT, FV_LEAVE))
+ j[DST_VEHICLE] = message.dstVehicle;
- if (messageType == messageTypes::LV_BROADCAST)
+ if (messageTypes::contains(messageType, LV_BROADCAST, FV_HEARTBEAT, LV_ACCEPT, FV_LEAVE))
+ j[PLATOON_ID] = message.platoonId;
+
+ if (messageTypes::contains(messageType, LV_BROADCAST))
{
j[PLATOON_SPEED] = message.platoonSpeed;
j[INNER_PLATOON_DISTANCE] = message.innerPlatoonDistance;
@@ -184,10 +196,15 @@ struct Decoder<platoonProtocol::PlatoonMessage>
using namespace jsonNames;
message.srcVehicle = j.at(SRC_VEHICLE).get<VehicleId>();
- message.dstVehicle = j.at(DST_VEHICLE).get<VehicleId>();
- message.platoonId = j.at(PLATOON_ID).get<PlatoonId>();
- if (messageType == messageTypes::LV_BROADCAST)
+ using namespace messageTypes;
+ if (messageTypes::contains(messageType, FV_HEARTBEAT, LV_ACCEPT, REJECT, FV_LEAVE))
+ message.dstVehicle = j.at(DST_VEHICLE).get<VehicleId>();
+
+ if (messageTypes::contains(messageType, LV_BROADCAST, FV_HEARTBEAT, LV_ACCEPT, FV_LEAVE))
+ message.platoonId = j.at(PLATOON_ID).get<VehicleId>();
+
+ if (messageTypes::contains(messageType, LV_BROADCAST))
{
message.platoonSpeed = j.at(PLATOON_SPEED).get<PlatoonSpeed>();
message.innerPlatoonDistance = j.at(INNER_PLATOON_DISTANCE).get<InnerPlatoonDistance>();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Protocol.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Protocol.h
index 898ce0080055a78dad067fa62197610569189a42..875dc8ae05116252b3ab4e4d0c510ddb71a7972c 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Protocol.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Protocol.h
@@ -22,6 +22,8 @@ using InnerPlatoonDistance = float;
struct PlatoonConfig
{
+ static constexpr float EPSILON{0.00001f};
+
PlatoonConfig() : platoonSpeed(0.0f), innerPlatoonDistance(0.0f)
{}
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
index f29de7d184415bd3ae476ad8283c875f7c33b93c..71a2358fa67735f800ca82d3f78f873ad8c8ed43 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/include/PlatoonProtocolLib/Vehicle.h
@@ -70,6 +70,7 @@ protected:
};
static const PlatoonConfig DEFAULT_PLATOON_CONFIG;
+ static const Callback DEFAULT_CALLBACK;
const NetworkInfo myInfo;
PlatoonId platoonId;
@@ -79,8 +80,8 @@ protected:
networking::Networking & net;
networking::message::DatagramReceiver<PlatoonMessage>::Ptr receiver;
networking::message::DatagramSender<PlatoonMessage>::Ptr sender;
- Callback onRunningPlatoonCallback;
- Callback onLeavingPlatoonCallback;
+ Callback onRunningPlatoonCallback{DEFAULT_CALLBACK};
+ Callback onLeavingPlatoonCallback{DEFAULT_CALLBACK};
virtual void doCreatePlatoon();
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
index 78f1027e1573d36cd745a92a56dc8b56c0851e28..130ae81a4394e22f5753f2d9ccbca71d87b0df36 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/FollowerVehicle.cpp
@@ -67,9 +67,9 @@ void FollowerVehicle::doLeavePlatoon()
if (state == State::IDLE)
return;
- Vehicle::doLeavePlatoon();
-
sendLeavePlatoonMessage();
+
+ Vehicle::doLeavePlatoon();
}
void FollowerVehicle::receiveMessage(const PlatoonMessage & message)
@@ -174,6 +174,8 @@ void FollowerVehicle::receiveBroadcast(const PlatoonMessage & broadcast)
void FollowerVehicle::receiveResponse(const PlatoonMessage & response)
{
+ log("response\n");
+
if (state != State::CREATING_PLATOON ||
response.dstVehicle != myInfo.vehicleId)
return;
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
index 6b6796bdcbf0e366eb302c9de307a1487ebfa214..a908df36624545a834fa07b938efbd1cf556e45a 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/LeaderVehicle.cpp
@@ -194,6 +194,8 @@ void LeaderVehicle::receiveLeavePlatoonMessage(const PlatoonMessage & message)
void LeaderVehicle::startBroadcasting()
{
+ sendBroadcastMessage(); // Send broadcast immediately.
+
auto self = shared_from_this();
broadcastTimer->startPeriodicTimeout(
BROADCAST_INTERVAL,
@@ -202,15 +204,7 @@ void LeaderVehicle::startBroadcasting()
if (self->state != State::RUNNING_PLATOON)
return;
- PlatoonConfig configCopy = self->platoonConfig.load();
-
- // 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);
+ self->sendBroadcastMessage();
});
}
@@ -219,6 +213,18 @@ void LeaderVehicle::stopBroadcasting()
broadcastTimer->stop();
}
+void LeaderVehicle::sendBroadcastMessage()
+{
+ PlatoonConfig configCopy = platoonConfig.load();
+ // Send the broadcast message and forget about it.
+ sendMessage(PlatoonMessage::broadcastMessage(
+ myInfo.vehicleId,
+ platoonId,
+ configCopy.platoonSpeed,
+ configCopy.innerPlatoonDistance,
+ utils::keys(followers)), BROADCAST_INTERVAL);
+}
+
void LeaderVehicle::removeAllFollowers()
{
utils::foreachInMap(followers,
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/Protocol.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/Protocol.cpp
index 3974d9b7d16a853bd24ef9f4d928d003834808a7..b53047c8b46ab3371981de8c435e80da124d592a 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/Protocol.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/Protocol.cpp
@@ -3,13 +3,15 @@
//
#include "../include/PlatoonProtocolLib/Protocol.h"
+#include <cmath>
namespace platoonProtocol
{
bool operator==(const PlatoonConfig & lhs, const PlatoonConfig & rhs) noexcept
{
- return lhs.platoonSpeed == rhs.platoonSpeed && lhs.innerPlatoonDistance == rhs.innerPlatoonDistance;
+ return std::fabs(lhs.platoonSpeed - rhs.platoonSpeed) < PlatoonConfig::EPSILON &&
+ std::fabs(lhs.innerPlatoonDistance - rhs.innerPlatoonDistance) < PlatoonConfig::EPSILON;
}
bool operator!=(const PlatoonConfig & lhs, const PlatoonConfig & rhs) noexcept
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
index 5d80ec68146edfa3ef54b6fcf867c9df64444280..4c1c1eb5805375ec6f8e3ea6018a6fdad4f9a352 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/src/Vehicle.cpp
@@ -12,6 +12,8 @@ namespace platoonProtocol
{
constexpr std::uint16_t Vehicle::UDP_PORT;
+const Vehicle::Callback Vehicle::DEFAULT_CALLBACK = []
+{};
const PlatoonConfig Vehicle::DEFAULT_PLATOON_CONFIG{0.0f, 0.0f};
Vehicle::Vehicle(networking::Networking & net,
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9fe2c35c785161d2cf85ed60ed09578d31e5cfc9
--- /dev/null
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp
@@ -0,0 +1,415 @@
+//
+// Created by philipp on 12.04.18.
+//
+
+#include "../include/PlatoonProtocolLib/Protocol.h"
+#include "../include/PlatoonProtocolLib/FollowerVehicle.h"
+#include "../include/PlatoonProtocolLib/LeaderVehicle.h"
+#include "NetworkingLib/Networking.h"
+#include <chrono>
+
+namespace platoonProtocol
+{
+
+networking::Networking net;
+const std::string broadcastAddress = "10.255.255.255";
+auto leader = LeaderVehicle::create(net, NetworkInfo{1, broadcastAddress});
+auto follower1 = FollowerVehicle::create(net, NetworkInfo{2, broadcastAddress});
+auto follower2 = FollowerVehicle::create(net, NetworkInfo{3, broadcastAddress});
+
+using namespace std::chrono_literals;
+const auto DEFAULT_WAIT_TOLERANCE = 30s;
+const auto TIMEOUT_TOLERANCE = 10ms;
+
+const PlatoonConfig TEST_CONFIG{1.0f, 10.0f};
+
+networking::time::Duration abs(networking::time::Duration d)
+{
+ return d >= d.zero() ? d : -d;
+}
+
+bool waitUntil(networking::time::Duration maxWaitDuration, const std::function<bool()> & condition)
+{
+ auto startTime = networking::time::now();
+ while (!condition() && networking::time::now() - startTime < maxWaitDuration);
+ return condition() && networking::time::now() - startTime < maxWaitDuration;
+}
+
+// -------------------
+// Test 1
+// -------------------
+
+bool test1Leader()
+{
+ leader->setPlatoonConfig(TEST_CONFIG);
+ leader->createPlatoon();
+ return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ { return leader->isPlatoonRunning(); });
+}
+
+bool test1Follower1()
+{
+ follower1->createPlatoon();
+ return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ { return follower1->isPlatoonRunning(); });
+}
+
+bool test1Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 2
+// -------------------
+
+bool test2Leader()
+{
+ if (!test1Leader())
+ return false;
+ // Give some time to send the first broadcast message.
+ std::this_thread::sleep_for(1s);
+ return true;
+}
+
+bool test2Follower1()
+{
+ follower1->createPlatoon();
+ return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ {
+ return follower1->isPlatoonRunning() &&
+ follower1->getPlatoonConfig() == TEST_CONFIG;
+ });
+}
+
+bool test2Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 3
+// -------------------
+
+bool test3Leader()
+{
+ if (!test1Leader())
+ return false;
+
+ // Wait a little bit so the first config can be send.
+ std::this_thread::sleep_for(30ms);
+
+ for (std::size_t i = 1; i <= 9; i++)
+ {
+ leader->setPlatoonConfig(PlatoonConfig{TEST_CONFIG.platoonSpeed + i * 0.1f,
+ TEST_CONFIG.innerPlatoonDistance + i});
+ std::this_thread::sleep_for(LeaderVehicle::BROADCAST_INTERVAL);
+ }
+
+ // Give some time to send the last broadcast message.
+ std::this_thread::sleep_for(50ms);
+ return true;
+}
+
+bool test3Follower1()
+{
+ std::atomic<bool> running{true};
+ std::atomic<bool> success{false};
+ auto expectedConfig = TEST_CONFIG;
+ auto lastConfigTime = networking::time::TimePoint::min();
+ std::size_t numConfigsReceived = 0;
+
+ follower1->setOnPlatoonConfigUpdatedCallback(
+ [&]
+ {
+ auto now = networking::time::now();
+ auto deltaTime = now - lastConfigTime;
+ lastConfigTime = now;
+
+ if (follower1->getPlatoonConfig() != expectedConfig)
+ {
+ success = false;
+ running = false;
+ return;
+ }
+
+ if (numConfigsReceived > 0 &&
+ abs(deltaTime - LeaderVehicle::BROADCAST_INTERVAL) > TIMEOUT_TOLERANCE)
+ {
+ success = false;
+ running = false;
+ return;
+ }
+
+ expectedConfig.platoonSpeed += 0.1f;
+ expectedConfig.innerPlatoonDistance += 1.0f;
+ numConfigsReceived++;
+ if (numConfigsReceived >= 10)
+ {
+ success = true;
+ running = false;
+ }
+ });
+
+ follower1->createPlatoon();
+
+ return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ { return !running; }) && success;
+}
+
+bool test3Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 4
+// -------------------
+
+bool test4Leader()
+{
+ if (!test1Leader())
+ return false;
+ std::this_thread::sleep_for(1s);
+ leader->stop();
+ return true;
+}
+
+bool test4Follower1()
+{
+ if (!test1Follower1())
+ return false;
+ return waitUntil(1s + FollowerVehicle::BROADCAST_TIMEOUT + TIMEOUT_TOLERANCE, [&]
+ { return !follower1->isPlatoonRunning(); });
+}
+
+bool test4Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 5
+// -------------------
+
+bool test5Leader()
+{
+ if (!test1Leader())
+ return false;
+ return waitUntil(1s + LeaderVehicle::HEARTBEAT_TIMEOUT + TIMEOUT_TOLERANCE, [&]
+ { return !leader->isPlatoonRunning(); });
+}
+
+bool test5Follower1()
+{
+ if (!test1Follower1())
+ return false;
+ std::this_thread::sleep_for(1s);
+ follower1->stop();
+ return true;
+}
+
+bool test5Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 6
+// -------------------
+
+bool test6Leader()
+{
+ if (!test1Leader())
+ return false;
+ return waitUntil(1100ms, [&]
+ { return !leader->isPlatoonRunning(); });
+}
+
+bool test6Follower1()
+{
+ if (!test1Follower1())
+ return false;
+ std::this_thread::sleep_for(1s);
+ follower1->leavePlatoon();
+ // Give some time to send leave platoon message.
+ std::this_thread::sleep_for(50ms);
+ return true;
+}
+
+bool test6Follower2()
+{
+ return true;
+}
+
+// -------------------
+// Test 7
+// -------------------
+
+bool test7Leader()
+{
+ return true;
+}
+
+bool test7Follower1()
+{
+ follower1->createPlatoon();
+ std::this_thread::sleep_for(5s);
+ return !follower1->isPlatoonRunning();
+}
+
+bool test7Follower2()
+{
+ follower2->createPlatoon();
+ std::this_thread::sleep_for(5s);
+ return !follower2->isPlatoonRunning();
+}
+
+// -------------------
+// Test 8
+// -------------------
+
+bool test8Leader()
+{
+ if (!test1Leader())
+ return false;
+ std::this_thread::sleep_for(100ms);
+ leader->setPlatoonConfig(PlatoonConfig{TEST_CONFIG.platoonSpeed + 0.1f, TEST_CONFIG.innerPlatoonDistance + 1.0f});
+ std::this_thread::sleep_for(100ms);
+ leader->setPlatoonConfig(PlatoonConfig{TEST_CONFIG.platoonSpeed + 0.2f, TEST_CONFIG.innerPlatoonDistance + 2.0f});
+ std::this_thread::sleep_for(100ms);
+ return true;
+}
+
+bool test8Followers(FollowerVehicle::Ptr follower)
+{
+ std::atomic<bool> success{false};
+ PlatoonConfig expectedConfig = TEST_CONFIG;
+ std::size_t numConfigsReceived = 0;
+ auto last = networking::time::now();
+
+ follower->setOnPlatoonConfigUpdatedCallback(
+ [&]
+ {
+ auto delta = networking::time::now() - last;
+ std::cout << "time passed [ms]: " << std::chrono::duration_cast<std::chrono::milliseconds>(delta).count() << "\n";
+ std::cout << "ps: " << follower->getPlatoonConfig().platoonSpeed << "\n";
+ if (follower->getPlatoonConfig() != expectedConfig)
+ {
+ success = false;
+ return;
+ }
+
+ expectedConfig.platoonSpeed += 0.1f;
+ expectedConfig.innerPlatoonDistance += 1.0f;
+ numConfigsReceived++;
+ if (numConfigsReceived >= 3)
+ success = true;
+ });
+
+ follower->createPlatoon();
+
+ return waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ { return success.load(); });
+}
+
+bool test8Follower1()
+{
+ return test8Followers(follower1);
+}
+
+bool test8Follower2()
+{
+ return test8Followers(follower2);
+}
+
+// -------------------
+// Test 9
+// -------------------
+
+bool test9Leader()
+{
+ if (!test1Leader())
+ return false;
+ std::this_thread::sleep_for(1100ms);
+ bool result = leader->isPlatoonRunning();
+ // Wait some time to signal that the platoon is still running.
+ std::this_thread::sleep_for(900ms);
+ return result;
+}
+
+bool test9Follower1()
+{
+ if (!test1Follower1())
+ return false;
+ std::this_thread::sleep_for(1s);
+ follower1->leavePlatoon();
+ // Give some time to send the leave platoon message.
+ std::this_thread::sleep_for(50ms);
+ return true;
+}
+
+bool test9Follower2()
+{
+ follower2->createPlatoon();
+ auto result = waitUntil(DEFAULT_WAIT_TOLERANCE, [&]
+ { return follower2->isPlatoonRunning(); });
+ if (!result)
+ return false;
+ std::this_thread::sleep_for(1100ms);
+ result = follower2->isPlatoonRunning();
+ // Wait some time to signal that the platoon is still running.
+ std::this_thread::sleep_for(900ms);
+ return result;
+}
+
+bool runTestScenario(int testNum, int testRole)
+{
+ using Test = bool (*)();
+ Test tests[9][3] = {
+ {test1Leader, test1Follower1, test1Follower2},
+ {test2Leader, test2Follower1, test2Follower2},
+ {test3Leader, test3Follower1, test3Follower2},
+ {test4Leader, test4Follower1, test4Follower2},
+ {test5Leader, test5Follower1, test5Follower2},
+ {test6Leader, test6Follower1, test6Follower2},
+ {test7Leader, test7Follower1, test7Follower2},
+ {test8Leader, test8Follower1, test8Follower2},
+ {test9Leader, test9Follower1, test9Follower2}
+ };
+ return tests[testNum - 1][testRole - 1]();
+}
+
+}
+
+int main(int argc, char ** argv)
+{
+ if (argc < 3)
+ {
+ std::cerr << "Too few arguments!\n";
+ return -1;
+ }
+
+ int testNum = atoi(argv[1]);
+ int testRole = atoi(argv[2]);
+
+ if (testNum < 1 || testNum > 9)
+ {
+ std::cerr << "First argument must be between 1 and 9!\n";
+ return -1;
+ }
+
+ if (testRole < 1 || testRole > 3)
+ {
+ std::cerr << "Second argument must be between 1 and 3";
+ return -1;
+ }
+
+ auto result = platoonProtocol::runTestScenario(testNum, testRole);
+ if (result)
+ std::cout << "1";
+ else
+ std::cout << "0";
+
+ return 0;
+}
\ No newline at end of file
diff --git a/modules/clean.sh b/modules/clean.sh
new file mode 100644
index 0000000000000000000000000000000000000000..508f1af5e6f8fd6066ccd140d4ce440f06e71d45
--- /dev/null
+++ b/modules/clean.sh
@@ -0,0 +1,8 @@
+rm -r catkin_ws/install
+rm -r catkin_ws/build
+rm -r catkin_ws/devel
+rm -r catkin_ws/src/car/cmake-build-debug
+rm -r catkin_ws/src/NetworkingLib/cmake-build-debug
+rm -r catkin_ws/src/PlatoonProtocolLib/cmake-build-debug
+rm -r catkin_ws/src/PC2CarLib/cmake-build-debug
+rm -r catkin_ws/src/PC/cmake-build-debug