Merge branch 'master' of https://github.com/F-WuTS/compLIB

added enabling of legacy support for the camera module to the postinstall script

README.md

@@ -1,7 +1,11 @@
 # compLIB
 
+Rewrite for ROS is the live packaged version since 18.07.2023.
+
 # Dependencies
 
+TODO: document
+
 ## Building documentation
 ```
 pip install sphinx-rtd-theme
@@ -19,6 +23,7 @@ pip install sphinx-rtd-theme
 + `DEBUG`, default="0", If set to != "0" (default), debug prints will be enabled
 + `API_URL`, default="http://localhost:5000/"
 + `API_FORCE`, default="", if set to !="" (default), it will replace the API_URL env variable
++ `FORCE_SEED`, default="-1", if set to !="-1" (default), the seeding seed supplied by the user will be ignored and this seed will be used instead
 
 # Stream Video
 
@@ -41,3 +46,5 @@ ffmpeg -f v4l2  -framerate 30 -video_size 640x480 -i /dev/video0 -b:v 2M -f flv
 # Bullseye now only supports libcamera
 
 https://www.raspberrypi.com/news/bullseye-camera-system/
+
+(This can still be mitigated by enabling "old camera support" in the raspi-config settings. (This is done automatically in the postinstallscript)

build.sh

@@ -4,7 +4,6 @@ mkdir output
 
 DEB="empty"
 
-cd client
 source build_deb.sh
 echo "Ran build deb, created: $DEB"
-mv $DEB ../output
+mv $DEB ./output

client/.idea/.gitignore

@@ -1,3 +0,0 @@
-# Default ignored files
-/shelf/
-/workspace.xml

client/.idea/client_s2.iml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<module type="PYTHON_MODULE" version="4">
-  <component name="NewModuleRootManager">
-    <content url="file://$MODULE_DIR$" />
-    <orderEntry type="inheritedJdk" />
-    <orderEntry type="sourceFolder" forTests="false" />
-  </component>
-</module>

client/.idea/inspectionProfiles/Project_Default.xml

@@ -1,12 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <profile version="1.0">
-    <option name="myName" value="Project Default" />
-    <inspection_tool class="PyUnresolvedReferencesInspection" enabled="true" level="WARNING" enabled_by_default="true">
-      <option name="ignoredIdentifiers">
-        <list>
-          <option value="list.__getitem__" />
-        </list>
-      </option>
-    </inspection_tool>
-  </profile>
-</component>

client/.idea/inspectionProfiles/profiles_settings.xml

@@ -1,6 +0,0 @@
-<component name="InspectionProjectProfileManager">
-  <settings>
-    <option name="USE_PROJECT_PROFILE" value="false" />
-    <version value="1.0" />
-  </settings>
-</component>

client/.idea/misc.xml

@@ -1,7 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.9" project-jdk-type="Python SDK" />
-  <component name="PyCharmProfessionalAdvertiser">
-    <option name="shown" value="true" />
-  </component>
-</project>

client/.idea/modules.xml

@@ -1,8 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="ProjectModuleManager">
-    <modules>
-      <module fileurl="file://$PROJECT_DIR$/.idea/client_s2.iml" filepath="$PROJECT_DIR$/.idea/client_s2.iml" />
-    </modules>
-  </component>
-</project>

client/.idea/saveactions_settings.xml

@@ -1,15 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="SaveActionSettings">
-    <option name="actions">
-      <set>
-        <option value="activate" />
-        <option value="activateOnShortcut" />
-        <option value="activateOnBatch" />
-        <option value="organizeImports" />
-        <option value="reformat" />
-        <option value="rearrange" />
-      </set>
-    </option>
-  </component>
-</project>

client/.idea/vcs.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<project version="4">
-  <component name="VcsDirectoryMappings">
-    <mapping directory="$PROJECT_DIR$/.." vcs="Git" />
-  </component>
-</project>

client/dev01.py

@@ -1,24 +0,0 @@
-import time
-
-from compLib.CompLibClient import CompLibClient
-
-
-def main():
-    from compLib.Motor import Motor
-
-    # Motor.speed(0, -50)
-    # Motor.speed(3, 50)
-
-    Motor.power(0, 50)
-    Motor.power(3, -50)
-
-    time.sleep(2)
-
-    Motor.power(0, 0)
-    Motor.power(3, -0)
-
-
-if __name__ == '__main__':
-    CompLibClient.use_tcp_socket("dev01.local")
-    # follow()
-    main()

client/dev03.py

@@ -1,34 +0,0 @@
-import time
-
-from compLib.CompLibClient import CompLibClient
-from compLib.IRSensor import IRSensor
-
-
-def main():
-    # Motor.speed(0, -50)
-    # Motor.speed(3, 50)
-
-    # Motor.power(0, 50)
-    # Motor.power(3, -50)
-    #
-    # time.sleep(2)
-    #
-    # Motor.power(0, 0)
-    # Motor.power(3, -0)
-
-    start_time = time.time()
-    for i in range(0, 1000):
-        IRSensor.read_all()
-        # Motor.multiple_power((0, 1), (3, 1))
-        # Motor.speed(0, 1)
-        # Motor.speed(3, 1)
-
-    print(1000.0 / (time.time() - start_time))
-
-
-if __name__ == '__main__':
-    # CompLibClient.use_tcp_socket("dev03.local")
-    CompLibClient.use_unix_socket()
-    # follow()
-    # cProfile.run("main()")
-    main()

client/docs/source/faq.rst

@@ -1,15 +0,0 @@
-FAQ
-###
-
-Was ist das Passwort für die Entwicklungsumgebung?
---------------------------------------------------
-``compair``
-
-Wie verbinde ich mich zur Entwicklungsumgebung?
------------------------------------------------
-
-See :ref:`gettingstarted_codeserver`
-
-Was ist der Benutzername und das Passwort für den Raspberry Pi?
----------------------------------------------------------------
-``compair`` ``compair``

client/lf.py

@@ -1,96 +0,0 @@
-from compLib.Motor import Motor
-from compLib.IRSensor import IRSensor
-from compLib.CompLibClient import CompLibClient
-
-import time
-import math
-
-DRIVE_SPEED = 20.0
-COLOR_BREAK = 850
-KP = 7.5
-KD = 0.0
-
-SAMPLE_TIME_S = 0.001
-CUTOFF_FREQ_HZ = 50.0
-
-RC = 1.0 / (2.0 * math.pi * CUTOFF_FREQ_HZ)
-
-FIRST_COEFF = SAMPLE_TIME_S / (SAMPLE_TIME_S + RC)
-SECOND_COEFF = RC / (SAMPLE_TIME_S + RC)
-
-out_old = 0.0
-start_time = time.time()
-
-
-def drive(leftSpeed, rightSpeed):
-    rightSpeed *= -0.906
-
-    Motor.multiple_power((0, min(max(-100, rightSpeed), 100)), (3, min(max(-100, leftSpeed), 100)))
-
-def follow(sleepTime = 0.3):
-    global out_old
-    lastError = 0
-    sensorsBlack = 0
-    while sensorsBlack < 3:
-        sensor_data = IRSensor.read_all()
-
-        sensorsBlack = 0
-        for i in range(0, 5):
-            if sensor_data[i] > COLOR_BREAK:
-                sensorsBlack += 1
-
-
-        middle_sensor = sensor_data[2]
-        filtered_sensor = FIRST_COEFF * middle_sensor + SECOND_COEFF * out_old
-        out_old = filtered_sensor
-
-        sample_time = str(time.time() - start_time).replace(".", ",")
-        print(f"{sample_time} {middle_sensor} {int(filtered_sensor)}")
-
-        error = lastError
-        if sensor_data[2] > COLOR_BREAK:
-            error = 0
-        elif sensor_data[0] > COLOR_BREAK:
-            error = -1.5
-        elif sensor_data[4] > COLOR_BREAK:
-            error = 1.5
-        elif sensor_data[1] > COLOR_BREAK:
-            error = -1
-        elif sensor_data[3] > COLOR_BREAK:
-            error = 1
-        elif error == 1.5:
-            error = 3
-        elif error == -1.5:
-            error = -3
-
-        lastError = error
-
-        adjustment = KP * error + KD * (error - lastError)
-        leftSpeed = DRIVE_SPEED + adjustment
-        rightSpeed = DRIVE_SPEED - adjustment
-
-        # print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
-        drive(leftSpeed, rightSpeed)
-
-        time.sleep(SAMPLE_TIME_S)
-
-    drive(0, 0)
-    time.sleep(sleepTime)
-
-def main():
-    CompLibClient.use_unix_socket()
-
-    IRSensor.enable()
-    time.sleep(0.5)
-
-    # while True:
-    #     print(IRSensor.read_all())
-    #     time.sleep(0.01)
-
-    follow()
-    follow()
-    follow()
-    follow()
-    follow(0.2)
-
-main()

client/main.py

@@ -1,174 +0,0 @@
-import time
-
-from compLib.CompLibClient import CompLibClient
-
-
-# def send(data, size):
-#     with socket.socket(socket.AF_UNIX, socket.SOCK_STREAM) as sock:
-#         sock.connect(SOCKET_PATH)
-#         sock.sendall(size.to_bytes(1, byteorder='big'))
-#         sock.sendall(data)
-#
-#         response_size_bytes = sock.recv(1)
-#         response_size = int.from_bytes(response_size_bytes, byteorder="big")
-#         # print(f"Response size: {response_size}")
-#
-#         response_bytes = sock.recv(response_size)
-#         generic_response = CompLib_pb2.GenericResponse()
-#
-#         generic_response.ParseFromString(response_bytes)
-#         # print(f"Response: {generic_response}")
-#
-#         # reponseBytes =
-
-
-def main():
-    # encoder_read_positions_request = CompLib_pb2.EncoderReadPositionsRequest()
-    # # readSensorsRequest.header = CompLib_pb2.Header()
-    # encoder_read_positions_request.header.message_type = encoder_read_positions_request.DESCRIPTOR.full_name
-    #
-    # start_time = time.time()
-    # for i in range(100000):
-    #     send(encoder_read_positions_request.SerializeToString(), encoder_read_positions_request.ByteSize())
-    # print("--- %s seconds ---" % (time.time() - start_time))
-
-    from compLib.IRSensor import IRSensor
-    IRSensor.enable()
-
-    startTime = time.time()
-    while time.time() - startTime < 10:
-        print(IRSensor.read_all())
-        time.sleep(0.01)
-
-    # from compLib.Encoder import Encoder
-    # print(Encoder.read_all_positions())
-    # print(Encoder.read_all_velocities())
-
-    # from compLib.Motor import Motor
-
-    # Motor.speed(0, -50)
-    # Motor.speed(3, 50)
-
-    # Motor.power(0, -50)
-    # Motor.power(3, 50)
-
-    # time.sleep(5)
-
-
-#
-# import time
-# time.sleep(2)
-#
-# Motor.speed(0, 0)
-# Motor.speed(3, -0)
-
-# Motor.power(0, 0)
-# Motor.power(3, 0)
-
-# import math
-# from compLib.Movement import Movement
-# Movement.turn_degrees(90, math.pi * 2)
-# Movement.turn_degrees(-90, math.pi * 2)
-#
-# Movement.turn_degrees(90, math.pi * 2)
-# Movement.turn_degrees(90, -math.pi * 2)
-#
-# Movement.turn_degrees(90, math.pi * 2)
-# Movement.turn_degrees(-90, -math.pi * 2)
-
-# from compLib.Movement import Movement
-# Movement.drive_distance(0.1, 0.5)
-# Movement.drive_distance(-0.1, 0.5)
-#
-# Movement.drive_distance(0.1, 0.5)
-# Movement.drive_distance(0.1, -0.5)
-#
-# Movement.drive_distance(0.1, 0.5)
-# Movement.drive_distance(-0.1, -0.5)
-
-# from compLib.Movement import Movement
-# import math
-# import time
-# Movement.drive_distance(0.5, 0.5)
-# time.sleep(1)
-# Movement.turn_degrees(90, math.pi * 2)
-# time.sleep(1)
-#
-# Movement.drive_distance(0.5, 0.5)
-# time.sleep(1)
-# Movement.turn_degrees(90, math.pi * 2)
-# time.sleep(1)
-#
-# Movement.drive_distance(0.5, 0.5)
-# time.sleep(1)
-# Movement.turn_degrees(90, math.pi * 2)
-# time.sleep(1)
-#
-# Movement.drive_distance(0.5, 0.5)
-# time.sleep(1)
-# Movement.turn_degrees(90, math.pi * 2)
-# time.sleep(1)
-
-# import time
-#
-# from compLib.IRSensor import IRSensor
-# from compLib.Motor import Motor
-#
-# IRSensor.enable()
-#
-# DRIVE_SPEED = 2.0
-# COLOR_BREAK = 900
-# KP = 0.25
-# KD = 0.0
-#
-#
-# def drive(leftSpeed, rightSpeed):
-#     Motor.speed(0, -rightSpeed)
-#     Motor.power(3, leftSpeed)
-#
-#
-# def follow(sleepTime=0.1):
-#     lastError = 0
-#     sensorsBlack = 0
-#
-#     while sensorsBlack < 3:
-#         data = IRSensor.read_all()
-#
-#         sensorsBlack = 0
-#         for i in range(len(data)):
-#             if data[i] > COLOR_BREAK:
-#                 sensorsBlack += 1
-#
-#         error = lastError
-#         if data[2] > COLOR_BREAK:
-#             error = 0
-#         elif data[0] > COLOR_BREAK:
-#             error = -1.5
-#         elif data[4] > COLOR_BREAK:
-#             error = 1.5
-#         elif data[1] > COLOR_BREAK:
-#             error = -1
-#         elif data[3] > COLOR_BREAK:
-#             error = 1
-#         elif error == 1.5:
-#             error = 3
-#         elif error == -1.5:
-#             error = -3
-#
-#         lastError = error
-#
-#         adjustment = KP * error + KD * (error - lastError)
-#         leftSpeed = DRIVE_SPEED + adjustment
-#         rightSpeed = DRIVE_SPEED - adjustment
-#
-#         print(f"{leftSpeed} {rightSpeed} {adjustment} {error}")
-#         drive(leftSpeed, rightSpeed)
-#
-#     drive(0, 0)
-#     time.sleep(sleepTime)
-
-
-if __name__ == '__main__':
-    CompLibClient.use_tcp_socket("dev03.local")
-    # follow()
-    main()

compLib/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.26)
+project(comp_lib)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(irobot_create_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+# add_executable(talker src/publisher_member_function.cpp)
+# ament_target_dependencies(talker rclcpp std_msgs irobot_create_msgs)
+
+# add_executable(listener src/subscriber_member_function.cpp)
+# ament_target_dependencies(listener rclcpp std_msgs irobot_create_msgs)
+
+# add_executable(drive src/drive_action.cpp)
+# ament_target_dependencies(drive rclcpp rclcpp_action std_msgs irobot_create_msgs)
+
+# add_executable(set_vel src/set_speed.cpp)
+# ament_target_dependencies(set_vel rclcpp rclcpp_action std_msgs irobot_create_msgs geometry_msgs)
+
+add_executable(compLib_node src/main.cpp src/motor.cpp src/controls.cpp)
+ament_target_dependencies(compLib_node rclcpp rclcpp_action std_msgs irobot_create_msgs geometry_msgs)
+
+target_include_directories(compLib_node PRIVATE include)
+
+install(TARGETS
+  #talker
+  #listener
+  #drive
+  #set_vel
+  compLib_node
+  DESTINATION lib/${PROJECT_NAME})
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

compLib/Camera.py

@@ -1,7 +1,5 @@
 import sys
-from typing import Tuple, Any
-
-import numpy as np
+from typing import Any, Tuple, List
 
 # build image is somehow different from raspberry image? opencv-python is installed to a directory which is not in the pythonpath by default....
 sys.path.append("/usr/lib/python3.9/site-packages")
@@ -60,6 +58,7 @@ class Camera:
             def __video_feed():
                 """
                 Define route for serving jpeg stream.
+
                 :return: Return the response generated along with the specific media.
                 """
                 return Response(self.__camera._newest_frame_generator(),
@@ -69,13 +68,15 @@ class Camera:
             def __index():
                 """
                 Define route for serving a static http site to view the stream.
-                :return: Static html page
+
+                :return: Static html page where the video stream of Opencv can be viewed.
                 """
                 return HTML
 
         def __start_flask(self):
             """
             Function for running flask server in a thread.
+
             :return:
             """
             logging.getLogger("complib-logger").info("starting flask server")
@@ -123,27 +124,31 @@ class Camera:
 
         self.__logger.info("Initialized vision")
 
-    def get_frame(self):
+    def get_frame(self) -> Any:
         """
-        Die Funktion das neuste Bild, welches die Kamera aufgenommen hat zurück.
+        Die Funktion gibt das neuste Bild, welches die Kamera aufgenommen, hat zurück.
+
         :return: Ein "opencv image frame"
         """
         img16 = self.__camera_stream.read()
         return img16
 
-    def detect_markers(self, image):
+    def detect_markers(self, image) -> Any:
         """
         Funktion um die ArUco Marker in einem Bild zu erkennen.
+
         :param image: Bild, welches die Kamera aufgenommen hat.
         :return: Gibt drei Variablen zurueck. Erstens eine Liste an Postionen der "Ecken" der erkannten Markern. Zweitens eine Liste an IDs der erkannten Markern und dritten noch Debug Informationen (diese können ignoriert werden).
         """
         return cv2.aruco.detectMarkers(image, self.aruco_dict, parameters=self.aruco_params)
 
-    def detect_markers_midpoint(self, image) -> tuple[list[Marker], Any]:
+    def detect_markers_midpoint(self, image) -> Tuple[List[Marker], Any]:
         """
         Funktion um die ArUco Marker in einem Bild zu erkennen, einzuzeichnen und den Mittelpunkt der Marker auszurechnen.
+
         :param image: Bild, welches die Kamera aufgenommen hat.
         :return: Gibt zwei Variablen zurueck. Erstens eine Liste an "Markern" und zweitens das Bild mit den eigezeichneten Marken.
+        :rtype: Tuple[List[Marker], Any]
         """
         (corners, ids, rejected) = self.detect_markers(image)
         self.draw_markers(image, corners, ids)
@@ -156,9 +161,10 @@ class Camera:
 
         return res, image
 
-    def draw_markers(self, image, corners, ids):
+    def draw_markers(self, image, corners, ids) -> Any:
         """
         Zeichnet die erkannten Markern mit ihren IDs in das Bild.
+
         :param image: Original Bild, in dem die Marker erkannt wurden.
         :param corners: List der Positionen der Ecken der erkannten Marker.
         :param ids: IDs der erkannten Markern.
@@ -169,6 +175,7 @@ class Camera:
     def publish_frame(self, image):
         """
         Sendet das Bild, welches der Funktion übergeben wird, an den Webserver, damit es der Nutzer in seinem Browser ansehen kann.
+
         :param image: Opencv Bild, welches dem Nutzer angezeigt werden soll.
         :return: None
         """
@@ -179,6 +186,7 @@ class Camera:
     def _newest_frame_generator(self):
         """
         Private generator which is called directly from flask server.
+
         :return: Yields image/jpeg encoded frames published from publish_frame function.
         """
         while True:

compLib/Motor.py

@@ -62,7 +62,7 @@ class Motor(object):
         """Geschwindigkeit des Motors einstellen
 
         :param port: Port, an welchen der Motor angesteckt ist. 0-3
-        :param speed: Drehzahl, mit der sich ein Motor dreht, in Radianten pro Sekunde (rad/s)
+        :param speed: Drehzahl, mit der sich ein Motor dreht, in Centimeter pro Sekunde (cm/s)
         :raises: IndexError
         """
 

compLib/Seeding.py

@@ -1,8 +1,10 @@
 import logging
+import os
 
 import numpy as np
 
 # TODO: if set to competition mode, get the seed from the api
+FORCE_SEED = int(os.getenv("FORCE_SEED", "-1"))
 
 logger = logging.getLogger("complib-logger")
 
@@ -32,9 +34,14 @@ Logistic Centers: {self.logistic_center}"""
         :param seed: Seed welcher zum Erstellen des Gamestates benutzt werden soll.
         """
 
-        logger.debug(f"Creating gamestate with seed: {seed}")
-        self.seed = seed
-        self.__set_random_seed(seed)
+        if FORCE_SEED == -1:
+            self.seed = seed
+        else:
+            print(f"Wettkampfmodus, zufälliger Seed wird verwendet: Seed={FORCE_SEED}")
+            self.seed = FORCE_SEED
+
+        logger.debug(f"Creating gamestate with seed: {self.seed}")
+        self.__set_random_seed(self.seed)
 
         self.heu_color = self.__get_random_number(1, 2)
 

compLib/include/comp_lib_node.h

@@ -0,0 +1,14 @@
+#ifndef ROS_NODE_H
+#define ROS_NODE_H
+
+#include "rclcpp/rclcpp.hpp"
+
+class CompLibNode : public rclcpp::Node
+{
+public:
+    CompLibNode();
+
+    
+};
+
+#endif

compLib/include/controls.h

@@ -0,0 +1,24 @@
+#ifndef CONTROLS_H
+#define CONTROLS_H
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "irobot_create_msgs/msg/interface_buttons.hpp"
+#include "irobot_create_msgs/msg/lightring_leds.hpp"
+
+class ButtonPressNode : public rclcpp::Node
+{
+public:
+    ButtonPressNode();
+    void bt1_wait();
+    void bt2_wait();
+    void kill();
+private:
+    void result_callback(const irobot_create_msgs::msg::InterfaceButtons::SharedPtr result);
+    rclcpp::Subscription<irobot_create_msgs::msg::InterfaceButtons>::SharedPtr interface_buttons_subscriber_;
+    bool button1{false};
+    bool button2{false};
+};
+
+#endif

compLib/include/motor.h

@@ -0,0 +1,67 @@
+#ifndef MOTOR_H
+#define MOTOR_H
+
+#include <thread>
+#include <memory>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "irobot_create_msgs/action/drive_distance.hpp"
+#include "irobot_create_msgs/action/drive_arc.hpp"
+#include "irobot_create_msgs/action/rotate_angle.hpp"
+
+class DriveDistNode : public rclcpp::Node
+{
+public:
+    DriveDistNode();
+    void drive_dist(float meters, float velocity);
+    void kill();
+private:
+    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SharedPtr drive_dist_action_;
+    bool processing;
+};
+
+class SetSpeedNode : public rclcpp::Node
+{
+public:
+    SetSpeedNode();
+    void drive(float speed);
+    void stop();
+    void kill();
+private:
+    void set_speed(float speed);
+    void drive_loop(float speed);
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr speed_publisher_;
+    bool run = true;
+    std::thread t;
+};
+
+class RotateAngleNode : public rclcpp::Node
+{
+public:
+    RotateAngleNode();
+    void rotate_angle(float angle, float velocity);
+    void kill();
+private:
+    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SharedPtr rotate_angle_action_;
+    bool processing;
+};
+
+class DriveArcNode : public rclcpp::Node
+{
+public:
+    DriveArcNode();
+    void drive_arc(float angle, float radius, float velocity, int direction=1);
+    void kill();
+private:
+    void result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SharedPtr drive_arc_action_;
+    bool processing;
+};
+
+#endif

compLib/include/sequence_lock.h

@@ -0,0 +1,11 @@
+#ifndef SEQUENCE_LOCK_H
+#define SEQUENCE_LOCK_H
+
+#include <mutex>
+
+namespace SequenceLock
+{
+    std::mutex m;
+}
+
+#endif

compLib/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>comp_lib</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="matthias@todo.todo">matthias</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>std_msgs</depend>
+  <depend>irobot_create_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

compLib/src/comp_lib_node.cpp

@@ -0,0 +1,10 @@
+#include "ros_node.h"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp" 
+
+CompLibNode::CompLibNode()
+: Node("CompLibNode")
+{
+
+}

compLib/src/controls.cpp

@@ -0,0 +1,53 @@
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "irobot_create_msgs/msg/interface_buttons.hpp"
+
+#include "controls.h"
+
+ButtonPressNode::ButtonPressNode()
+: Node("button_press_node")
+{
+    interface_buttons_subscriber_ = this->create_subscription<irobot_create_msgs::msg::InterfaceButtons>(
+        "/interface_buttons",
+        rclcpp::SensorDataQoS(),
+        std::bind(&ButtonPressNode::result_callback, this, std::placeholders::_1)
+    );
+}
+
+void ButtonPressNode::bt1_wait()
+{
+    RCLCPP_INFO(this->get_logger(), "Wait for button 1...");
+    button1 = false;
+    while (!button1) {}
+    button1 = false;
+}
+
+void ButtonPressNode::bt2_wait()
+{
+    RCLCPP_INFO(this->get_logger(), "Wait for button 2...");
+    button2 = false;
+    while (!button2) {}
+    button2 = false;
+}
+
+void ButtonPressNode::result_callback(const irobot_create_msgs::msg::InterfaceButtons::SharedPtr result)
+{
+    if (result->button_1.is_pressed) {
+        button1 = true;
+    }
+
+    if (result->button_2.is_pressed) {
+        button2 = true;
+    }
+
+    if (result->button_power.is_pressed) {
+
+    }
+}
+
+void ButtonPressNode::kill()
+{
+    RCLCPP_INFO(this->get_logger(), "ButtonPressNode killed");
+    rclcpp::shutdown();
+}

compLib/src/main.cpp

@@ -0,0 +1,93 @@
+#include <thread>
+#include <memory>
+#include <chrono>
+#include <mutex>
+
+#include "motor.h"
+#include "controls.h"
+
+std::mutex action_mutex;
+
+// #lazyness
+void run_node(std::shared_ptr<DriveDistNode> node)
+{
+    rclcpp::spin(node);
+}
+
+void run_node1(std::shared_ptr<SetSpeedNode> node)
+{
+    rclcpp::spin(node);
+}
+
+void run_node2(std::shared_ptr<RotateAngleNode> node)
+{
+    rclcpp::spin(node);
+}
+
+void run_node3(std::shared_ptr<DriveArcNode> node)
+{
+    rclcpp::spin(node);
+}
+
+void run_node4(std::shared_ptr<ButtonPressNode> node)
+{
+    rclcpp::spin(node);
+}
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+
+    auto ddn = std::make_shared<DriveDistNode>();
+    auto ssn = std::make_shared<SetSpeedNode>();
+    auto ran = std::make_shared<RotateAngleNode>();
+    auto dan = std::make_shared<DriveArcNode>();
+    auto bpn = std::make_shared<ButtonPressNode>();
+    
+    std::thread t;
+    std::thread t1;
+    std::thread t2;
+    std::thread t3;
+    std::thread t4;
+
+    t = std::thread(run_node, ddn);
+    t1 = std::thread(run_node1, ssn);
+    t2 = std::thread(run_node2, ran);
+    t3 = std::thread(run_node3, dan);
+    t4 = std::thread(run_node4, bpn);
+
+    bpn->bt1_wait();
+    bpn->bt2_wait();
+    bpn->bt1_wait();
+
+    ssn->drive(0.3);
+
+    std::this_thread::sleep_for (std::chrono::milliseconds(2000));
+
+    ssn->stop();
+
+    // std::this_thread::sleep_for (std::chrono::milliseconds(1000));
+
+    ran->rotate_angle(-45, 0.5);
+    // std::this_thread::sleep_for (std::chrono::milliseconds(5000));
+
+    ddn->drive_dist(0.2, 0.3);
+
+    // std::this_thread::sleep_for (std::chrono::milliseconds(5000));
+
+    dan->drive_arc(90, 0.5, 0.5);
+
+    ddn->kill();
+    ssn->kill();
+    ddn->kill();
+    dan->kill();
+    bpn->kill();
+
+    t.join();
+    t1.join();
+    t2.join();
+    t3.join();
+    t4.join();
+
+    return 0;
+}

compLib/src/motor.cpp

@@ -0,0 +1,225 @@
+#include <chrono>
+#include <thread>
+#include <mutex>
+#include <future>
+#include <memory>
+
+#include <geometry_msgs/msg/twist.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+#include "irobot_create_msgs/action/drive_distance.hpp"
+#include "irobot_create_msgs/action/drive_arc.hpp"
+#include "irobot_create_msgs/action/rotate_angle.hpp"
+
+#include "motor.h"
+#include "sequence_lock.h"
+
+double pi = 2 * acos(0.0);
+
+DriveDistNode::DriveDistNode()
+: Node("drive_dist_node")
+{
+    drive_dist_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveDistance>(
+        this,
+        "/drive_distance"
+    );
+}
+
+void DriveDistNode::drive_dist(float meters, float velocity)
+{
+    RCLCPP_INFO(this->get_logger(), "drive dist");
+    processing = true;
+    auto data = irobot_create_msgs::action::DriveDistance::Goal();
+    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveDistance>::SendGoalOptions();
+    send_goal_options.result_callback =
+        std::bind(&DriveDistNode::result_callback, this, std::placeholders::_1);
+
+    data.distance = meters;
+    data.max_translation_speed = velocity;
+    drive_dist_action_->async_send_goal(data, send_goal_options);
+
+    while (processing) {}
+}
+
+void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::WrappedResult & result)
+{
+    processing = false;
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            RCLCPP_INFO(this->get_logger(), "finished dist");
+            return;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+            return;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            return;
+    }
+}
+
+void DriveDistNode::kill()
+{
+    RCLCPP_INFO(this->get_logger(), "DriveDistNode killed");
+    rclcpp::shutdown();
+}
+
+SetSpeedNode::SetSpeedNode()
+: Node("set_speed_node")
+{
+    speed_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>(
+        "/cmd_vel",
+        rclcpp::SensorDataQoS()
+    );
+}
+
+void SetSpeedNode::drive_loop(float speed)
+{
+    while (run)
+    {
+        set_speed(speed);
+        // sleep set as described at http://wiki.ros.org/Robots/TIAGo/Tutorials/motions/cmd_vel
+        std::this_thread::sleep_for (std::chrono::milliseconds(333));
+    }
+}
+
+void SetSpeedNode::set_speed(float speed)
+{
+    auto data = geometry_msgs::msg::Twist();
+
+    data.linear.x = speed;
+    data.linear.y = 0;
+    data.linear.z = 0;
+    data.angular.x = 0;
+    data.angular.x = 0;
+    data.angular.x = 0;
+    speed_publisher_->publish(data);
+}
+
+void SetSpeedNode::drive(float speed)
+{
+    RCLCPP_INFO(this->get_logger(), "Start drive");
+    run = true;
+    t = std::thread(&SetSpeedNode::drive_loop, this, speed);
+}
+
+void SetSpeedNode::stop()
+{
+    run = false;
+    RCLCPP_INFO(this->get_logger(), "Stop drive");
+}
+
+void SetSpeedNode::kill()
+{
+    RCLCPP_INFO(this->get_logger(), "SetSpeedNode killed");
+    rclcpp::shutdown();
+}
+
+RotateAngleNode::RotateAngleNode()
+: Node("rotate_angle_node")
+{
+    rotate_angle_action_ = rclcpp_action::create_client<irobot_create_msgs::action::RotateAngle>(
+        this,
+        "rotate_angle"
+    );
+}
+
+void RotateAngleNode::rotate_angle(float angle, float velocity)
+{
+    processing = true;
+    angle *= pi / 180;
+    auto data = irobot_create_msgs::action::RotateAngle::Goal();
+    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::RotateAngle>::SendGoalOptions();
+    send_goal_options.result_callback =
+        std::bind(&RotateAngleNode::result_callback, this, std::placeholders::_1);
+    
+
+    data.angle = angle;
+    data.max_rotation_speed = velocity;
+    rotate_angle_action_->async_send_goal(data, send_goal_options);
+
+    while (processing) {}
+}
+
+void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::RotateAngle>::WrappedResult & result)
+{
+    processing = false;
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            RCLCPP_INFO(this->get_logger(), "finished rotation");
+            return;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+            return;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            return;
+    }
+}
+
+void RotateAngleNode::kill()
+{
+    RCLCPP_INFO(this->get_logger(), "RotateAngleNode killed");
+    rclcpp::shutdown();
+}
+
+DriveArcNode::DriveArcNode()
+: Node("drive_arc_node")
+{
+    drive_arc_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveArc>(
+        this,
+        "/drive_arc"
+    );
+}
+
+
+void DriveArcNode::drive_arc(float angle, float radius, float velocity, int direction)
+{
+    processing = true;
+    angle *= pi / 180;
+    auto data = irobot_create_msgs::action::DriveArc::Goal();
+    auto send_goal_options = rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SendGoalOptions();
+    send_goal_options.result_callback =
+        std::bind(&DriveArcNode::result_callback, this, std::placeholders::_1);
+    
+
+    data.angle = angle;
+    data.radius = radius;
+    data.translate_direction = direction;
+    data.max_translation_speed = velocity;
+    drive_arc_action_->async_send_goal(data, send_goal_options);
+
+    while (processing) {}
+}
+
+void DriveArcNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveArc>::WrappedResult & result)
+{
+    processing = false;
+    switch (result.code) {
+        case rclcpp_action::ResultCode::SUCCEEDED:
+            RCLCPP_INFO(this->get_logger(), "finished arc");
+            return;
+        case rclcpp_action::ResultCode::ABORTED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
+            return;
+        case rclcpp_action::ResultCode::CANCELED:
+            RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
+            return;
+        default:
+            RCLCPP_ERROR(this->get_logger(), "Unknown result code");
+            return;
+    }
+}
+
+void DriveArcNode::kill()
+{
+    RCLCPP_INFO(this->get_logger(), "DriveArcNode killed");
+    rclcpp::shutdown();
+}

docs/source/faq.rst

@@ -0,0 +1,32 @@
+FAQ
+###
+
+Was ist das Passwort für die Entwicklungsumgebung?
+--------------------------------------------------
+``compair``
+
+Wie verbinde ich mich zur Entwicklungsumgebung?
+-----------------------------------------------
+
+See :ref:`gettingstarted_codeserver`
+
+Was ist der Benutzername und das Passwort für den Raspberry Pi?
+---------------------------------------------------------------
+``compair`` ``compair``
+
+Wie aktualisiere ich meine Software?
+------------------------------------
+
+.. code-block:: bash
+
+    sudo apt update
+    sudo apt upgrade
+    sudo update-firmware
+
+Wie kann ich die SD-Karte neu beschreiben?
+------------------------------------------
+`SD-Karten Image <https://drive.google.com/drive/folders/16lMe-yGphk947L4WPjd4oD8ndY9R1WbA?usp=share_link>`_
+
+Software zum Schreiben der SD-Karte `balenaEtcher <https://www.balena.io/etcher/>`_
+
+

docs/source/gettingStarted/index.rst

@@ -7,5 +7,6 @@ Erste Schritte
     wifi.rst
     codeServer.rst
     firstProgram.rst
+    update.rst
     secondProgram.rst
     thridProgram.rst

docs/source/gettingStarted/update.rst

@@ -0,0 +1,13 @@
+Software Updaten
+#################
+
+Da wir die ``compLib``, und die andere Software, welche auf dem Roboter läuft, laufend weiterentwickeln, solltet ihr immer wieder euren Roboter auf die neuste Version updaten. Dazu müsst ihr einfach den Roboter mit dem Internet verbinden und dann diesen Befehl in der Kommandozeile des Roboters eingeben:
+
+.. code-block:: bash
+
+    sudo apt update && sudo apt upgrade
+
+Am einfachsten kann das über die Webseite gemacht werden, auf der ihr auch euren Code schreibt. Dazu müsst ihr einfach nur das Terminal (= Konsole) öffnen, dann den Befehl dort hineinkopieren und Enter drücken.
+|updatePic|
+
+.. |updatePic| image:: images/09_update.png

docs/source/lib/classes/Opencv.rst

@@ -0,0 +1,79 @@
+.. _lib_camera:
+
+Camera und OpenCV
+*******************
+
+Dokumentation des Camera Moduls
+================================
+
+.. autoclass:: compLib.Camera.Marker
+   :members:
+
+.. autoclass:: compLib.Camera.Camera
+   :members:
+
+Beispiele
+=========
+
+Bild Anzeigen
+---------------
+
+Das folgende Programm fragt Bilder von der Kamera ab und schickt sie an den Webserver, der im Hintergrund läuft. Der Benutzer kann dann auf die Webseite: http://raspi_ip:9898 gehen und die Ausgabe der Kamera sehen.
+
+.. code-block:: python
+
+    from compLib.Camera import *
+
+    camera = Camera()
+    while True:
+        image = camera.get_frame()
+        camera.publish_frame(image)
+
+ArUco Marker Erkennen
+------------------------
+
+In diesem Programm werden die ArUco Marker, die sich am Spielfeld befinden, erkannt. Diese "QR-Code" ähnlichen Marker finden sich in den Logistikzonen und können dazu verwendet werden zu erkennen, wo der Roboter hinfahren sollt etc.
+
+.. code-block:: python
+
+    from compLib.Camera import *
+
+    camera = Camera()
+    while True:
+        image = camera.get_frame()
+
+        markers, image = camera.detect_markers_midpoint(image)
+        print(markers)
+        print("-----------------")
+
+        camera.publish_frame(image)
+
+Hier ist z.B. der ArUco Marker mit der ID 0. Führe das Programm aus und lass den Roboter auf den Bildschirm schauen. Das Programm sollte die 2D Position ausgeben, welcher der ArUco Marker (genauer sein Mittelpunkt) im Camera Bild hat.
+
+|ArucoExample|
+
+.. |ArucoExample| image:: images/6x6_1000-0.png
+
+Um die Positionen zu verarbeiten, muss dann nur noch das "markers" array durchgegangen werden. Das könnte z.B. so gemacht werden:
+
+.. code-block:: python
+
+    from compLib.Camera import *
+
+    camera = Camera()
+    while True:
+        image = camera.get_frame()
+
+        markers, image = camera.detect_markers_midpoint(image)
+        print(markers)
+        print("-----------------")
+
+        for marker in markers:
+            print(f"Marker mit der id: {marker.id}")
+            print(f"Ist auf der X Position: {marker.x}")
+            print(f"und auf der Y Position: {marker.y}")
+            print("-----------------")
+
+        camera.publish_frame(image)
+
+Wichtig ist noch zu beachten, dass die X und Y Positionen ihren Ursprung in der linken oberen Ecke des Bildes haben. D.h. die Position (0,0) ist im oberen linken Bildrand.

docs/source/lib/index.rst

@@ -9,3 +9,4 @@ compLib
     classes/IRSensor
     classes/Seeding
     classes/DoubleElimination
+    classes/Opencv

postinstall.sh

@@ -1,3 +1,10 @@
+#!/usr/bin/env bash
+
+if [ "$(uname -m)" = "x86_64" ]; then
+  echo "Not running on RPi - Skipping postinstall"
+  exit 0
+fi
+
 grep -qxF "apt update" /etc/rc.local
 if [ $? -ne 0 ]; then
   echo "adding apt update to rc.local"
@@ -6,8 +13,10 @@ fi
 
 pip3 install requests flask
 
-echo "Setting up nginx rtmp server"
-sudo /etc/init.d/nginx start
+#echo "Setting up nginx rtmp server"
+#sudo /etc/init.d/nginx start
+
+sudo raspi-config nonint do_legacy 0 || echo "(WARNING) raspi-config not found, cannot enable legacy camera support"
 
 {
   echo 'load_module "modules/ngx_rtmp_module.so";'
@@ -26,6 +35,9 @@ sudo /etc/init.d/nginx start
   echo '}'
 } >| /etc/nginx/nginx.conf
 
+echo "Stopping nginx rtmp server as its not required anymore"
+sudo /etc/init.d/nginx stop
+
 base64 -d << UPD
 CiBfX19fX18gICAgIF9fX19fXyAgICAgX18gICAgX18gICAgIF9fX19fXyAgIF9fICAgICAgICAgX18gICAgIF9fX19fXyAgICAgICAgICAgICAgICAgIAovXCAgX19fXCAgIC9cICBfXyBcICAgL1wgIi0uLyAgXCAgIC9cICA9PSBcIC9cIFwgICAgICAgL1wgXCAgIC9cICA9PSBcICAgICAgICAgICAgICAgICAKXCBcIFxfX19fICBcIFwgXC9cIFwgIFwgXCBcLS4vXCBcICBcIFwgIF8tLyBcIFwgXF9fX18gIFwgXCBcICBcIFwgIF9fPCAgICAgICAgICAgICAgICAgCiBcIFxfX19fX1wgIFwgXF9fX19fXCAgXCBcX1wgXCBcX1wgIFwgXF9cICAgIFwgXF9fX19fXCAgXCBcX1wgIFwgXF9fX19fXCAgICAgICAgICAgICAgIAogIFwvX19fX18vICAgXC9fX19fXy8gICBcL18vICBcL18vICAgXC9fLyAgICAgXC9fX19fXy8gICBcL18vICAgXC9fX19fXy8gICAgICAgICAgICAgICAKICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgICAgCiBfXyAgX18gICAgIF9fX19fXyAgICAgIF9fX19fXyAgIF9fX19fXyAgICAgICAgX19fX18gICAgIF9fX19fXyAgICAgX19fX19fICAgX19fX19fICAgIAovXCBcL1wgXCAgIC9cICA9PSBcICAgIC9cX18gIF9cIC9cICBfXyBcICAgICAgL1wgIF9fLS4gIC9cICBfXyBcICAgL1xfXyAgX1wgL1wgIF9fX1wgICAKXCBcIFxfXCBcICBcIFwgIF8tLyAgICBcL18vXCBcLyBcIFwgXC9cIFwgICAgIFwgXCBcL1wgXCBcIFwgIF9fIFwgIFwvXy9cIFwvIFwgXCAgX19cICAgCiBcIFxfX19fX1wgIFwgXF9cICAgICAgICAgXCBcX1wgIFwgXF9fX19fXCAgICAgXCBcX19fXy0gIFwgXF9cIFxfXCAgICBcIFxfXCAgXCBcX19fX19cIAogIFwvX19fX18vICAgXC9fLyAgICAgICAgICBcL18vICAgXC9fX19fXy8gICAgICBcL19fX18vICAgXC9fL1wvXy8gICAgIFwvXy8gICBcL19fX19fLyA=
 UPD

setup.py

@@ -6,7 +6,7 @@ print("Using version: {str(os.environ['VERSION'])}")
 
 setuptools.setup(
     name="complib",
-    version=str(os.environ["VERSION"]),
+    version=str(os.environ.get('VERSION', "")),
     author="F-WuTs",
     author_email="joel.klimont@comp-air.at",
     description="",

test.py

@@ -61,6 +61,60 @@ class SeedingApiTest(unittest.TestCase):
             self.assertEqual(seeding_api.get_logistic_plan(), gamestate.get_logistic_plan())
             self.assertEqual(seeding_api.get_material_deliveries(), gamestate.get_material_deliveries())
 
+    def test_gamestate(self):
+        seed = 42
+        gamestate = Seeding.Gamestate(seed)
+        print(gamestate)
+
+        print(gamestate.get_heuballen())
+        heu_color = gamestate.get_heuballen()
+        if heu_color == 1:
+            print("Heuballen liegen auf den gelben Linien")
+            # TODO: code um die über die gelben Linien zu fahren
+        elif heu_color == 2:
+            print("Heuballen liegen auf den blauen Linien")
+            # TODO: code um die über die blauen Linien zu fahren
+
+        materials = gamestate.get_material_deliveries()
+        print(materials)
+
+        for material_pair in materials:
+            print(f"Der Roboter sollte jetzt die beiden Materialien {material_pair} holen")
+
+            for material in material_pair:
+                if material == 0:
+                    print(f"Der Roboter sollte jetzt Holz aufnehmen, Zone: {material}")
+                    # TODO: code um in die Material Zone mit dem Holz zu fahren
+                elif material == 1:
+                    print(f"Der Roboter sollte jetzt Stahl aufnehmen, Zone: {material}")
+                    # TODO: code um in die Material Zone mit dem Holz zu fahren
+                elif material == 2:
+                    print(f"Der Roboter sollte jetzt Beton aufnehmen, Zone: {material}")
+                    # TODO: code um in die Material Zone mit dem Holz zu fahren
+                elif material == 3:
+                    print(f"Der Roboter sollte jetzt Ziegelsteine aufnehmen, Zone: {material}")
+                    # TODO: code um in die Material Zone mit dem Holz zu fahren
+
+            print("Der Roboter sollte jetzt die beiden Materialien zur Baustelle fahren")
+            # TODO: code um zur Baustelle zu fahren
+
+        logistic_plan = gamestate.get_logistic_plan()
+        print(logistic_plan)
+
+        for zone in logistic_plan:
+            if zone == 10:
+                print(f"Roboter sollte jetzt zur grünen Zone fahren: {zone}")
+                # TODO: code um in die grüne Zone zu fahren
+            elif zone == 11:
+                print(f"Roboter sollte jetzt zur roten Zone fahren: {zone}")
+                # TODO: code um in die rote Zone zu fahren
+            elif zone == 12:
+                print(f"Roboter sollte jetzt zur blauen Zone fahren: {zone}")
+                # TODO: code um in die blaue Zone zu fahren
+            elif zone == 13:
+                print(f"Roboter sollte jetzt zur gelben Zone fahren: {zone}")
+                # TODO: code um in die gelbe Zone zu fahren
+
 
 class DeApiTest(unittest.TestCase):
     def test_api_de(self):
@@ -92,6 +146,5 @@ class DeApiTest(unittest.TestCase):
         self.assertTrue(util_reset_state())
 
 
-
 if __name__ == '__main__':
     unittest.main()