diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
index f4d3440a1f9fb0ba6185b55dda84ad40c3077e7d..acc56617a87ceac40af4bd7323c365b651c84c52 100644
--- a/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/CMakeLists.txt
@@ -145,11 +145,14 @@ endif()
 #######
 # Test
 #######
-set(TEST_SOURCE_FILES ${SOURCE_FILES} test/ProtocolTest.h test/ProtocolTest.cpp test/Main.cpp)
-add_executable(PlatoonProtocolTest ${TEST_SOURCE_FILES})
 
-target_link_libraries(PlatoonProtocolTest NetworkingLib)
-target_include_directories(PlatoonProtocolTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
+add_executable(LeaderTest ${SOURCE_FILES} test/LeaderTest.cpp)
+target_link_libraries(LeaderTest NetworkingLib)
+target_include_directories(LeaderTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
+
+add_executable(FollowerTest ${SOURCE_FILES} test/FollowerTest.cpp)
+target_link_libraries(FollowerTest NetworkingLib)
+target_include_directories(FollowerTest PUBLIC ${NetworkingLib_INCLUDE_DIRS})
 
 # For debugging
-target_compile_options(PlatoonProtocolTest PUBLIC -fopenmp -fPIC -O0 -g3 -ggdb)
\ No newline at end of file
+#target_compile_options(LeaderTest PUBLIC -fopenmp -fPIC -O0 -g3 -ggdb)
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6e4cd0fc4b6bf4a8a03742f97e6c794d8bdd53cc
--- /dev/null
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/FollowerTest.cpp
@@ -0,0 +1,46 @@
+//
+// Created by philipp on 03.04.18.
+//
+
+#include <NetworkingLib/Networking.h>
+#include "../include/PlatoonProtocolLib/Protocol.h"
+#include "../include/PlatoonProtocolLib/FollowerVehicle.h"
+
+int main(int argc, char ** argv)
+{
+    using namespace platoonProtocol;
+
+    if (argc < 3)
+    {
+        std::cerr << "Too few arguments!\n";
+        return -1;
+    }
+
+    networking::Networking net;
+
+    auto vehicleId = static_cast<VehicleId>(atoi(argv[1]));
+    std::string broadcastAddress{argv[2]};
+
+    auto follower = FollowerVehicle::create(net, NetworkInfo{vehicleId, broadcastAddress});
+    follower->setOnRunningPlatoonCallback([]
+                                          { std::cout << "Platoon is running!\n"; });
+    follower->setOnLeavingPlatoonCallback([]
+                                          { std::cout << "Left platoon!\n"; });
+    follower->setOnPlatoonConfigUpdatedCallback(
+        [follower]
+        {
+            auto config = follower->getPlatoonConfig();
+            std::cout << "Config Update:\nIPD=" << config.innerPlatoonDistance
+                      << "\nPS=" << config.platoonSpeed << "\n";
+        });
+
+    auto numVehiclesToRequest = argc - 3;
+    for (std::size_t i = 0; i < numVehiclesToRequest; i++)
+        follower->addVehicleToRequest(static_cast<VehicleId>(atoi(argv[i + 3])));
+
+    follower->createPlatoon();
+
+    while (1);
+
+    return 0;
+}
\ No newline at end of file
diff --git a/modules/catkin_ws/src/PlatoonProtocolLib/test/LeaderTest.cpp b/modules/catkin_ws/src/PlatoonProtocolLib/test/LeaderTest.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9d620dc5196d41998d064442d442a5e0de90f3bf
--- /dev/null
+++ b/modules/catkin_ws/src/PlatoonProtocolLib/test/LeaderTest.cpp
@@ -0,0 +1,40 @@
+//
+// Created by philipp on 03.04.18.
+//
+
+#include <NetworkingLib/Networking.h>
+#include "../include/PlatoonProtocolLib/Protocol.h"
+#include "../include/PlatoonProtocolLib/LeaderVehicle.h"
+
+int main(int argc, char ** argv)
+{
+    using namespace platoonProtocol;
+
+    if (argc < 3)
+    {
+        std::cerr << "Too few arguments!\n";
+        return -1;
+    }
+
+    networking::Networking net;
+
+    auto vehicleId = static_cast<VehicleId>(atoi(argv[1]));
+    std::string broadcastAddress{argv[2]};
+
+    auto leader = LeaderVehicle::create(net, NetworkInfo{vehicleId, broadcastAddress});
+    leader->setOnRunningPlatoonCallback([]{ std::cout << "Platoon is running!\n"; });
+    leader->setOnLeavingPlatoonCallback([]{ std::cout << "Left platoon!\n"; });
+    leader->createPlatoon();
+
+    float ps = 0.0f, ipd = 0.0f;
+
+    while (1)
+    {
+        leader->setPlatoonConfig(PlatoonConfig{ps, ipd});
+        ps += 1.0f;
+        ipd += 0.1f;
+        sleep(1);
+    }
+
+    return 0;
+}
\ No newline at end of file