Merge branch 'master' of gitlab.informatik.hu-berlin.de:badenhop/Hochautomatisiertes-Fahren

* 'master' of gitlab.informatik.hu-berlin.de:badenhop/Hochautomatisiertes-Fahren: (31 commits)
  clean script
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  -
  ...

Scripts/TestMaster/TestMaster.py

+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

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

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();
 

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;

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>();

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)
     {}
 

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();
 

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;

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,

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

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,

modules/catkin_ws/src/PlatoonProtocolLib/test/TestScenarios.cpp

+//
+// 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

modules/clean.sh

+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