diff --git a/client/.gitignore b/.gitignore similarity index 100% rename from client/.gitignore rename to .gitignore diff --git a/client/MANIFEST.in b/MANIFEST.in similarity index 100% rename from client/MANIFEST.in rename to MANIFEST.in diff --git a/README.md b/README.md index 0fbb89e..33f9085 100644 --- a/README.md +++ b/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 @@ -14,6 +18,13 @@ pip install sphinx-rtd-theme [Inline documentation example](https://pythonhosted.org/an_example_pypi_project/sphinx.html#full-code-example) [reStructured Text](https://pythonhosted.org/an_example_pypi_project/sphinx.html#restructured-text-rest-resources) +# ENV Variables + ++ `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 ``` @@ -31,3 +42,9 @@ with ffmpeg, cpu friendly ``` ffmpeg -f v4l2 -framerate 30 -video_size 640x480 -i /dev/video0 -b:v 2M -f flv rtmp://10.20.86.88/live/stream ``` + +# 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) diff --git a/build.sh b/build.sh index 657ddf0..670cd27 100644 --- a/build.sh +++ b/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 diff --git a/client/build_deb.sh b/build_deb.sh similarity index 94% rename from client/build_deb.sh rename to build_deb.sh index 493a241..bc29780 100755 --- a/client/build_deb.sh +++ b/build_deb.sh @@ -33,7 +33,9 @@ fpm -s python --python-bin python3 --python-package-name-prefix python3 \ -d "opencv-main" \ -d "opencv-python" \ -d "opencv-scripts" \ - -d "compsrv" \ + -d "libprotobuf23" \ + -d "protobuf-compiler" \ + -d "python3-protobuf" \ --python-install-lib "/usr/local/lib/python3.9/dist-packages" \ -v $VERSION -t deb setup.py @@ -56,4 +58,4 @@ echo "Created: $DEB" # sudo apt purge python3-complib -y # sudo apt install ./python3-* # sudo apt search complib -# ar vx ./python3* \ No newline at end of file +# ar vx ./python3* diff --git a/client/.idea/.gitignore b/client/.idea/.gitignore deleted file mode 100644 index 26d3352..0000000 --- a/client/.idea/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml diff --git a/client/.idea/client_s2.iml b/client/.idea/client_s2.iml deleted file mode 100644 index d0876a7..0000000 --- a/client/.idea/client_s2.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/client/.idea/inspectionProfiles/Project_Default.xml b/client/.idea/inspectionProfiles/Project_Default.xml deleted file mode 100644 index 9277599..0000000 --- a/client/.idea/inspectionProfiles/Project_Default.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - \ No newline at end of file diff --git a/client/.idea/inspectionProfiles/profiles_settings.xml b/client/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2d..0000000 --- a/client/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/client/.idea/misc.xml b/client/.idea/misc.xml deleted file mode 100644 index f6104af..0000000 --- a/client/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/client/.idea/modules.xml b/client/.idea/modules.xml deleted file mode 100644 index 8417644..0000000 --- a/client/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/client/.idea/saveactions_settings.xml b/client/.idea/saveactions_settings.xml deleted file mode 100644 index b7b3f34..0000000 --- a/client/.idea/saveactions_settings.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/client/.idea/vcs.xml b/client/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/client/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/client/compLib/__init__.py b/client/compLib/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/client/dev01.py b/client/dev01.py deleted file mode 100644 index 6ebfb8c..0000000 --- a/client/dev01.py +++ /dev/null @@ -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() diff --git a/client/dev03.py b/client/dev03.py deleted file mode 100644 index d4ed808..0000000 --- a/client/dev03.py +++ /dev/null @@ -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() diff --git a/client/docs/source/faq.rst b/client/docs/source/faq.rst deleted file mode 100644 index 513a48a..0000000 --- a/client/docs/source/faq.rst +++ /dev/null @@ -1,12 +0,0 @@ -FAQ -### - -Was ist das Passwort für die Entwicklungsumgebung? --------------------------------------------------- -``compair`` - -Wie verbinde ich mich zur Entwicklungsumgebung? ------------------------------------------------ - -See :ref:`gettingstarted_codeserver` - diff --git a/client/docs/source/lib/index.rst b/client/docs/source/lib/index.rst deleted file mode 100644 index 5a96950..0000000 --- a/client/docs/source/lib/index.rst +++ /dev/null @@ -1,8 +0,0 @@ -compLib -####### - -.. toctree:: - :maxdepth: 5 - :glob: - - classes/* diff --git a/client/docs/source/other/usage.rst b/client/docs/source/other/usage.rst deleted file mode 100644 index 0296e59..0000000 --- a/client/docs/source/other/usage.rst +++ /dev/null @@ -1,32 +0,0 @@ -.. _other_usage: - -Beispiele -######### - -Vorwärts und rückwärts fahren -***************************** - -.. code-block:: python - - import time - from compLib.Motor import * - - def forward(): - Motor.power(1, -30); - Motor.power(2, 30); - - - def backward(): - Motor.power(1, 30); - Motor.power(2, -30); - - def main(): - print("hallo ich bin ein roboter beep buup") - - forward() - time.sleep(1) - backward() - time.sleep(1) - - if __name__ == '__main__': - main() \ No newline at end of file diff --git a/client/docs/source/software/installation.rst b/client/docs/source/software/installation.rst deleted file mode 100644 index fb3b6eb..0000000 --- a/client/docs/source/software/installation.rst +++ /dev/null @@ -1,12 +0,0 @@ -.. _software_installation: - -Installationsanweisungen -######################## - -Diese Anleitung dient dazu die Software auf dem Roboter neu aufzusetzen. Im normalen Gebraucht sollte dies jedoch nicht notwendig sein. - -Betriebssystem aufsetzen -======================== - -Als Basis wird für den Roboter Raspberry Pi OS (64-bit) verwendet. Das 32-Bit Betriebssystem wird nicht unterstützt, da die Software-Komponenten nur für aarch64 bzw. arm64/v8 kompiliert werden. -Genauere Informationen sind `hier `_ zu finden. \ No newline at end of file diff --git a/client/lf.py b/client/lf.py deleted file mode 100644 index 7476dc9..0000000 --- a/client/lf.py +++ /dev/null @@ -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() \ No newline at end of file diff --git a/client/main.py b/client/main.py deleted file mode 100644 index 7e75006..0000000 --- a/client/main.py +++ /dev/null @@ -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() diff --git a/client/compLib/.gitignore b/compLib/.gitignore similarity index 100% rename from client/compLib/.gitignore rename to compLib/.gitignore diff --git a/client/compLib/Api.py b/compLib/Api.py similarity index 81% rename from client/compLib/Api.py rename to compLib/Api.py index 68c124a..f21ed57 100644 --- a/client/compLib/Api.py +++ b/compLib/Api.py @@ -5,7 +5,7 @@ from typing import Dict, Tuple, List import requests -logger = logging.getLogger("seeding-api") +logger = logging.getLogger("complib-logger") API_URL = os.getenv("API_URL", "http://localhost:5000/") + "api/" CONF_URL = os.getenv("API_URL", "http://localhost:5000/") + "config/" @@ -23,14 +23,14 @@ API_URL_GET_ROBOT_STATE = API_URL + "getRobotState" class Seeding: - """Class used for communicating with seeding api + """Klasse welche mit der Seeding API Kommuniziert. """ @staticmethod def get_heuballen() -> int: - """Makes the /api/getHeuballen call to the api. + """Macht den /api/getHeuballen request zur Seeding API. - :return: hueballencode as int. + :return: hueballencode als int. :rtype: int """ res = requests.get(API_URL_GET_HEU) @@ -40,9 +40,9 @@ class Seeding: @staticmethod def get_logistic_plan() -> List: - """Makes the /api/getLogisticPlan call to the api. + """Macht den /api/getLogisticPlan zur Seeding API. - :return: Json Object and status code as returned by the api. + :return: Liste an logistic-centern, welche vom roboter in genau der Reihenfolge beliefert werden sollten. :rtype: List """ res = requests.get(API_URL_GET_LOGISTIC_PLAN) @@ -52,7 +52,7 @@ class Seeding: @staticmethod def get_material_deliveries() -> List: - """Makes the /api/getMaterialDeliveries call to the api. + """Macht den /api/getMaterialDeliveries zur Seeding API. :return: Json Object and status code as returned by the api. :rtype: List diff --git a/compLib/CMakeLists.txt b/compLib/CMakeLists.txt new file mode 100644 index 0000000..d539099 --- /dev/null +++ b/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( 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() diff --git a/compLib/Camera.py b/compLib/Camera.py new file mode 100644 index 0000000..0937ec1 --- /dev/null +++ b/compLib/Camera.py @@ -0,0 +1,231 @@ +import sys +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") + +import logging +import os +import queue +import threading + +import cv2 +from flask import Flask, Response + +logging.basicConfig(format='%(asctime)s - %(name)s - %(levelname)s - %(message)s', level=logging.INFO) + +SERVE_VIDEO = os.getenv("SERVER_SRC", "/live") +BUILDING_DOCS = os.getenv("BUILDING_DOCS", "false") + +HTML = """ + + + Opencv Output + + +

Opencv Output

+ + + +""" + +# it would be better to use jinja2 here, but I don't want to blow up the package dependencies... +HTML = HTML.replace("{{ VIDEO_DST }}", SERVE_VIDEO) + + +class Marker: + def __init__(self, id: int, x: float, y: float): + self.id: int = id + self.x: float = x + self.y: float = y + + def __str__(self) -> str: + return f"Marker ID: {self.id}, position: {self.x} x, {self.y} y" + + def __repr__(self) -> str: + return str({"id": self.id, "x": self.x, "y": self.y}) + + +class Camera: + class __Webserver: + def __init__(self, camera): + self.app = Flask(__name__) + self.__camera = camera + self.__thread = threading.Thread(target=self.__start_flask, daemon=True) + self.__thread.start() + + @self.app.route("/live") + 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(), + mimetype="multipart/x-mixed-replace; boundary=frame") + + @self.app.route("/") + def __index(): + """ + Define route for serving a static http site to view the stream. + + :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") + self.app.run(host="0.0.0.0", port=9898, debug=True, threaded=True, use_reloader=False) + + class __NoBufferVideoCapture: + def __init__(self, cam): + self.cap = cv2.VideoCapture(cam) + self.cap.set(3, 640) + self.cap.set(4, 480) + self.q = queue.Queue(maxsize=3) + self.stopped = False + self.t = threading.Thread(target=self._reader, daemon=True) + self.t.start() + + def _reader(self): + while not self.stopped: + ret, frame = self.cap.read() + if not ret: + continue + if self.q.full(): + try: + self.q.get_nowait() + except queue.Empty: + pass + self.q.put(frame) + + def read(self): + return self.q.get() + + def stop(self): + self.stopped = True + self.t.join() + + def __init__(self): + self.__logger = logging.getLogger("complib-logger") + self.__logger.info("capturing rtmp stream is disabled in this version") + self.__camera_stream = self.__NoBufferVideoCapture(-1) + self.__newest_frame = None + self.__lock = threading.Lock() + self.__webserver = self.__Webserver(self) + + self.aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_50) + self.aruco_params = cv2.aruco.DetectorParameters_create() + + self.__logger.info("Initialized vision") + + def get_frame(self) -> Any: + """ + 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) -> 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]: + """ + 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) + + res = [] + for i in range(0, len(corners)): + x = sum([point[0] for point in corners[i][0]]) / 4 + y = sum([point[1] for point in corners[i][0]]) / 4 + res.append(Marker(ids[i][0], x, y)) + + return res, image + + 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. + :return: Neues Bild mit den eigezeichneten Markern. + """ + return cv2.aruco.drawDetectedMarkers(image, corners, ids) + + 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 + """ + with self.__lock: + if image is not None: + self.__newest_frame = image.copy() + + 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: + # use a buffer frame to copy the newest frame with lock and then freeing it immediately + buffer_frame = None + with self.__lock: + if self.__newest_frame is None: + continue + + buffer_frame = self.__newest_frame.copy() + + # encode frame for jpeg stream + (flag, encoded_image) = cv2.imencode(".jpg", buffer_frame) + + # if there was an error try again with the next frame + if not flag: + continue + + # else yield encoded frame with mimetype image/jpeg + yield (b'--frame\r\n' b'Content-Type: image/jpeg\r\n\r\n' + + bytearray(encoded_image) + b'\r\n') + + +# for debugging and testing start processing frames and detecting a 6 by 9 calibration chessboard +if __name__ == '__main__' and BUILDING_DOCS == "false": + camera = Camera() + while True: + image = camera.get_frame() + + criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) + + # processing + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + # find the chessboard corners + # ret, corners = cv2.findChessboardCorners(gray, (6, 9), None) + # cv2.drawChessboardCorners(frame, (6, 9), corners, ret) + + markers, image = camera.detect_markers_midpoint(image) + print(markers) + print("-----------------") + + camera.publish_frame(image) diff --git a/client/compLib/CompLib.proto b/compLib/CompLib.proto similarity index 100% rename from client/compLib/CompLib.proto rename to compLib/CompLib.proto diff --git a/client/compLib/CompLibClient.py b/compLib/CompLibClient.py similarity index 100% rename from client/compLib/CompLibClient.py rename to compLib/CompLibClient.py diff --git a/client/compLib/CompLib_pb2.py b/compLib/CompLib_pb2.py similarity index 100% rename from client/compLib/CompLib_pb2.py rename to compLib/CompLib_pb2.py diff --git a/client/compLib/DoubleElimination.py b/compLib/DoubleElimination.py similarity index 77% rename from client/compLib/DoubleElimination.py rename to compLib/DoubleElimination.py index 9cb4ae1..94f15ec 100644 --- a/client/compLib/DoubleElimination.py +++ b/compLib/DoubleElimination.py @@ -3,16 +3,10 @@ import os import time from typing import Tuple, List, Dict -import requests as requests +import requests import logging -logger = logging.getLogger("seeding-api") - -ch = logging.StreamHandler() -ch.setLevel(logging.DEBUG) -formatter = logging.Formatter("%(asctime)s - %(name)s - %(levelname)s - %(message)s") -ch.setFormatter(formatter) -logger.addHandler(ch) +logger = logging.getLogger("complib-logger") RETRY_TIMEOUT = 0.05 @@ -22,7 +16,7 @@ API_URL = os.getenv("API_URL", "http://localhost:5000/") + "api/" api_override = os.getenv("API_FORCE", "") if api_override != "": - print(f"API_URL was set to {API_URL} but was overwritten with {api_override}") + logger.warning(f"API_URL was set to {API_URL} but was overwritten with {api_override}") API_URL = api_override API_URL_GET_ROBOT_STATE = API_URL + "getRobotState" @@ -35,7 +29,12 @@ API_URL_GET_SCORES = API_URL + "getScores" class Position: - """Datastructure for holding a position + """ + Datenstruktur, welche eine Position representiert. + + :ivar x: X Position in Centimeter + :ivar y: Y Position in Centimeter + :ivar degrees: Rotation in Grad von -180 bis 180 """ def __init__(self, x, y, degrees): @@ -63,13 +62,14 @@ class Position: class DoubleElim: - """Class used for communicating with double elimination api + """Klasse für die Kommunikation mit Double Elimination Api """ @staticmethod def get_pos() -> Tuple[Position, int]: - """Makes the /api/getPos call to the api. - :return: A Position object with robot position + """Führt den /api/getPos Aufruf an die API aus. + + :return: Ein Objekt der Klasse :class:`.Position` mit der Position des Roboters und der Status Code :rtype: Tuple[Position, int] """ res = requests.get(API_URL_GET_POS) @@ -86,8 +86,9 @@ class DoubleElim: @staticmethod def get_opponent() -> Tuple[Position, int]: - """Makes the /api/getOp call to the api. - :return: A Position object with opponents robot position + """Führt den /api/getOp Aufruf an die API aus. + + :return: Ein Objekt der Klasse :class:`.Position` mit der Position des gegnerischen Roboters relativ zum eigenen Roboter und der Status Code :rtype: Tuple[Position, int] """ res = requests.get(API_URL_GET_OP) @@ -104,8 +105,9 @@ class DoubleElim: @staticmethod def get_goal() -> Tuple[Position, int]: - """Makes the /api/getGoal call to the api. - :return: A Position object with x and y coordinates of the goal, rotation is always -1 + """Führt den /api/getGoal Aufruf an die API aus. + + :return: Ein Objekt der Klasse :class:`.Position` mit der Position des Ziels relativ zum eigenen Roboter und der Status Code :rtype: Tuple[Position, int] """ res = requests.get(API_URL_GET_GOAL) @@ -122,8 +124,9 @@ class DoubleElim: @staticmethod def get_items() -> Tuple[List[Dict], int]: - """Makes the /api/getItems call to the api. - :return: A list will all items currently on the game field. Items are dictionaries that look like: {"id": 0, "x": 0, "y": 0} + """Führt den /api/getItems Aufruf an die API aus. + + :return: Eine Liste aller Items, die sich derzeit auf dem Spielfeld befinden. Items sind "dictionaries", die wie folgt aussehen: {"id": 0, "x": 0, "y": 0} :rtype: Tuple[List[Dict], int] """ res = requests.get(API_URL_GET_ITEMS) @@ -140,8 +143,9 @@ class DoubleElim: @staticmethod def get_scores() -> Tuple[Dict, int]: - """Makes the /api/getScores call to the api. - :return: A dictionary with all scores included like: {"self":2,"opponent":0} + """Führt den /api/getScores Aufruf an die API aus. + + :return: Ein "dictionary" mit dem eignen Score und dem des Gegners: {"self":2,"opponent":0} :rtype: Tuple[Dict, int] """ res = requests.get(API_URL_GET_SCORES) diff --git a/client/compLib/Encoder.py b/compLib/Encoder.py similarity index 56% rename from client/compLib/Encoder.py rename to compLib/Encoder.py index d1cf404..b292e41 100644 --- a/client/compLib/Encoder.py +++ b/compLib/Encoder.py @@ -3,33 +3,35 @@ from compLib.CompLibClient import CompLibClient class Encoder(object): - """Class used to read the encoders + """Klasse zum Zugriff auf die Encoder der einzelnen Motoren """ @staticmethod def read_all_positions(): - """Read all encoder positions. + """Lesen aller absoluten Positionen der einzelnen Encoder - :return: Tuple of all current encoder positions + :return: Tupel mit allen aktuellen Encoderpositionen """ request = CompLib_pb2.EncoderReadPositionsRequest() request.header.message_type = request.DESCRIPTOR.full_name response = CompLib_pb2.EncoderReadPositionsResponse() - response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + response.ParseFromString(CompLibClient.send( + request.SerializeToString(), request.ByteSize())) return tuple(i for i in response.positions) @staticmethod def read_all_velocities(): - """Read the velocity of all motors connected. + """Lesen der Geschwindigkeit aller angeschlossenen Motoren. - :return: Tuple of all current motor velocities + :return: Tupel aller aktuellen Motorgeschwindigkeiten in Radianten pro Sekunde """ request = CompLib_pb2.EncoderReadVelocitiesRequest() request.header.message_type = request.DESCRIPTOR.full_name response = CompLib_pb2.EncoderReadVelocitiesResponse() - response.ParseFromString(CompLibClient.send(request.SerializeToString(), request.ByteSize())) + response.ParseFromString(CompLibClient.send( + request.SerializeToString(), request.ByteSize())) return tuple(i for i in response.velocities) diff --git a/client/compLib/HealthCheck.py b/compLib/HealthCheck.py similarity index 100% rename from client/compLib/HealthCheck.py rename to compLib/HealthCheck.py diff --git a/client/compLib/IRSensor.py b/compLib/IRSensor.py similarity index 79% rename from client/compLib/IRSensor.py rename to compLib/IRSensor.py index ce7e29d..01a2bc9 100644 --- a/client/compLib/IRSensor.py +++ b/compLib/IRSensor.py @@ -6,14 +6,14 @@ from compLib.CompLibClient import CompLibClient class IRSensor(object): - """Access the different IR Sensors of the robot + """Ermöglicht den Zugriff auf die einzelnen IRSensoren des Roboters """ @staticmethod def read_all(): - """Read all IR sensors at once. + """Auslesen aller Sensoren gleichzeitig - :return: Array of all current ir sensors + :return: Array aller Sensorwerte """ request = CompLib_pb2.IRSensorsReadAllRequest() request.header.message_type = request.DESCRIPTOR.full_name @@ -26,7 +26,7 @@ class IRSensor(object): @staticmethod def enable(): - """Turn on all IR emitters + """Aktivieren Infrarot-Sender. Muss bei jedem Programmstart ausgeführt werden. """ request = CompLib_pb2.IRSensorsEnableRequest() request.header.message_type = request.DESCRIPTOR.full_name @@ -36,7 +36,7 @@ class IRSensor(object): @staticmethod def disable(): - """Turn off all IR emitters + """Deaktivieren der Infrarot-Sender """ request = CompLib_pb2.IRSensorsDisableRequest() request.header.message_type = request.DESCRIPTOR.full_name diff --git a/client/compLib/Motor.py b/compLib/Motor.py similarity index 93% rename from client/compLib/Motor.py rename to compLib/Motor.py index 4a05e44..3529ba1 100644 --- a/client/compLib/Motor.py +++ b/compLib/Motor.py @@ -12,7 +12,7 @@ class Motor(object): def power(port: int, percent: float): """Motor auf eine prozentuale Leistung der Höchstgeschwindigkeit einstellen - :param port: Port, an welchen der Motor angestecht wird. 0-3 + :param port: Port, an welchen der Motor angesteckt ist. 0-3 :param percent: Prozentsatz der Höchstgeschwindigkeit. zwischen -100 und 100 :raises: IndexError """ @@ -61,8 +61,8 @@ class Motor(object): def speed(port: int, speed: float): """Geschwindigkeit des Motors einstellen - :param port: Port, an welchen der Motor angestecht wird. 0-3 - :param speed: Drehzahl, mit der sich ein Motor dreht, in Radianten pro Sekunde (rad/s) + :param port: Port, an welchen der Motor angesteckt ist. 0-3 + :param speed: Drehzahl, mit der sich ein Motor dreht, in Centimeter pro Sekunde (cm/s) :raises: IndexError """ @@ -103,7 +103,7 @@ class Motor(object): def pulse_width(port: int, percent: float): """Setzen den Pulsbreite eines Motors in Prozent der Periode - :param port: Port, an welchen der Motor angestecht wird. 0-3 + :param port: Port, an welchen der Motor angesteckt ist. 0-3 :param percent: Prozent der Periode zwischen -100 und 100 :raises: IndexError """ @@ -120,7 +120,7 @@ class Motor(object): @staticmethod def multiple_pulse_width(*arguments: tuple[int, float]): - """Setzen den Pulsbreite mehreer Motoren in Prozent der Periode + """Setzen den Pulsbreite mehrerer Motoren in Prozent der Periode :param arguments: tuple von port, prozent :raises: IndexError diff --git a/client/compLib/Movement.py b/compLib/Movement.py similarity index 100% rename from client/compLib/Movement.py rename to compLib/Movement.py diff --git a/client/compLib/Seeding.py b/compLib/Seeding.py similarity index 52% rename from client/compLib/Seeding.py rename to compLib/Seeding.py index ab49a21..2eae33e 100644 --- a/client/compLib/Seeding.py +++ b/compLib/Seeding.py @@ -1,17 +1,24 @@ +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")) -def set_random_seed(seed: int): - np.random.seed(seed) - - -def get_random_number(min: int, max: int): - return np.random.randint(256 ** 4, dtype=' str: return f"""Seed: {self.seed} Heu Color: {self.heu_color} @@ -21,18 +28,30 @@ Logistic Plan: {self.logistic_plan} Logistic Centers: {self.logistic_center}""" def __init__(self, seed: int): - self.seed = seed - set_random_seed(seed) + """ + Erstellt den Seeding "Gamestate" für den angegebenen Seed. - self.heu_color = get_random_number(1, 2) + :param seed: Seed welcher zum Erstellen des Gamestates benutzt werden soll. + """ + + 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) self.materials = [0, 0, 0, 0] self.material_pairs = [] for i in range(0, 4): - num1 = get_random_number(0, 3) + num1 = self.__get_random_number(0, 3) self.material_pairs.append([num1, num1]) while self.material_pairs[i][1] == num1: - self.material_pairs[i][1] = get_random_number(0, 3) + self.material_pairs[i][1] = self.__get_random_number(0, 3) flat = [item for sublist in self.material_pairs for item in sublist] for i in range(0, 4): @@ -43,7 +62,7 @@ Logistic Centers: {self.logistic_center}""" visited = [5, 5, 5, 5] def __logistic_plan_generator(i: int): - drive_to = get_random_number(0, 3) + drive_to = self.__get_random_number(0, 3) for j in range(0, 4): drive_to = (drive_to + j) % 4 if visited[drive_to] <= 0 or drive_to == self.logistic_plan[i - 1]: @@ -78,12 +97,28 @@ Logistic Centers: {self.logistic_center}""" self.logistic_center[self.logistic_plan[i]][self.logistic_plan[i + 1]] += 1 self.logistic_plan = [x + 10 for x in self.logistic_plan] + logger.debug(f"Created gamesate: {str(self)}") def get_heuballen(self) -> int: + """ + Die Funktion gibt entweder die Zahl "1" oder "2" zurück. Wenn die Funktion "1" zurückgibt, dann liegen die Heuballen auf den gelben Linien. Wenn die Funktion "2" zurückgibt, dann liegen sie auf den blauen Flächen. + + :return: Gibt entweder die Zahl 1 oder 2 zurück. + """ return self.heu_color def get_logistic_plan(self) -> []: + """ + Die Funktion gibt den "Logistik Plan" zurück. Also die Reihenfolge, in welcher der Roboter die Logistik Zonen Abfahren muss, um die Pakete welche dort liegen zu sortieren. + + :return: Eine Liste an Zahlen zwischen 10 und 13. + """ return self.logistic_plan def get_material_deliveries(self) -> [[]]: + """ + Die Funktion gibt die einzelnen "Material Lieferungen" zurück. Da der Roboter immer zwei Paare an Materialien anliefern muss, gibt die Funktion eine Liste an Material Paaren zurück. Die Materialien werden dabei durch ihre Zonen-ID representiert. Also Holz ist z.B. "0" und die Ziegelsteine sind "3". + + :return: Eine Liste and Material Paaren. + """ return self.material_pairs diff --git a/compLib/__init__.py b/compLib/__init__.py new file mode 100644 index 0000000..708874e --- /dev/null +++ b/compLib/__init__.py @@ -0,0 +1,7 @@ +import logging +import os + +if os.getenv("DEBUG", "0") != "0": + logging.basicConfig(format='%(asctime)s - %(name)s - %(levelname)s - %(message)s', level=logging.DEBUG) +else: + logging.basicConfig(format='%(asctime)s - %(name)s - %(levelname)s - %(message)s', level=logging.INFO) diff --git a/compLib/include/comp_lib_node.h b/compLib/include/comp_lib_node.h new file mode 100644 index 0000000..271c19a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/compLib/include/controls.h b/compLib/include/controls.h new file mode 100644 index 0000000..24a3780 --- /dev/null +++ b/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::SharedPtr interface_buttons_subscriber_; + bool button1{false}; + bool button2{false}; +}; + +#endif \ No newline at end of file diff --git a/compLib/include/motor.h b/compLib/include/motor.h new file mode 100644 index 0000000..a2096c0 --- /dev/null +++ b/compLib/include/motor.h @@ -0,0 +1,67 @@ +#ifndef MOTOR_H +#define MOTOR_H + +#include +#include + +#include + +#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::WrappedResult & result); + rclcpp_action::Client::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::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::WrappedResult & result); + rclcpp_action::Client::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::WrappedResult & result); + rclcpp_action::Client::SharedPtr drive_arc_action_; + bool processing; +}; + +#endif \ No newline at end of file diff --git a/compLib/include/sequence_lock.h b/compLib/include/sequence_lock.h new file mode 100644 index 0000000..a74dd14 --- /dev/null +++ b/compLib/include/sequence_lock.h @@ -0,0 +1,11 @@ +#ifndef SEQUENCE_LOCK_H +#define SEQUENCE_LOCK_H + +#include + +namespace SequenceLock +{ + std::mutex m; +} + +#endif \ No newline at end of file diff --git a/compLib/package.xml b/compLib/package.xml new file mode 100644 index 0000000..b05fe71 --- /dev/null +++ b/compLib/package.xml @@ -0,0 +1,22 @@ + + + + comp_lib + 0.0.0 + TODO: Package description + matthias + TODO: License declaration + + ament_cmake + rclcpp + rclcpp_action + std_msgs + irobot_create_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/compLib/src/comp_lib_node.cpp b/compLib/src/comp_lib_node.cpp new file mode 100644 index 0000000..6052379 --- /dev/null +++ b/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") +{ + +} \ No newline at end of file diff --git a/compLib/src/controls.cpp b/compLib/src/controls.cpp new file mode 100644 index 0000000..a4a84a5 --- /dev/null +++ b/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( + "/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(); +} \ No newline at end of file diff --git a/compLib/src/main.cpp b/compLib/src/main.cpp new file mode 100644 index 0000000..5c6296f --- /dev/null +++ b/compLib/src/main.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include + +#include "motor.h" +#include "controls.h" + +std::mutex action_mutex; + +// #lazyness +void run_node(std::shared_ptr node) +{ + rclcpp::spin(node); +} + +void run_node1(std::shared_ptr node) +{ + rclcpp::spin(node); +} + +void run_node2(std::shared_ptr node) +{ + rclcpp::spin(node); +} + +void run_node3(std::shared_ptr node) +{ + rclcpp::spin(node); +} + +void run_node4(std::shared_ptr node) +{ + rclcpp::spin(node); +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + auto ddn = std::make_shared(); + auto ssn = std::make_shared(); + auto ran = std::make_shared(); + auto dan = std::make_shared(); + auto bpn = std::make_shared(); + + 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; +} \ No newline at end of file diff --git a/compLib/src/motor.cpp b/compLib/src/motor.cpp new file mode 100644 index 0000000..a0e4528 --- /dev/null +++ b/compLib/src/motor.cpp @@ -0,0 +1,225 @@ +#include +#include +#include +#include +#include + +#include + +#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( + 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::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::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( + "/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( + 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::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::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( + 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::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::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(); +} \ No newline at end of file diff --git a/client/docs/.gitignore b/docs/.gitignore similarity index 100% rename from client/docs/.gitignore rename to docs/.gitignore diff --git a/client/docs/Makefile b/docs/Makefile similarity index 100% rename from client/docs/Makefile rename to docs/Makefile diff --git a/client/docs/make.bat b/docs/make.bat similarity index 100% rename from client/docs/make.bat rename to docs/make.bat diff --git a/client/docs/source/_static/.gitkeep b/docs/source/_static/.gitkeep similarity index 100% rename from client/docs/source/_static/.gitkeep rename to docs/source/_static/.gitkeep diff --git a/client/docs/source/conf.py b/docs/source/conf.py similarity index 93% rename from client/docs/source/conf.py rename to docs/source/conf.py index 7e9fc73..081dd2b 100644 --- a/client/docs/source/conf.py +++ b/docs/source/conf.py @@ -22,6 +22,7 @@ os.environ["EXTENSIVE_LOGGING"] = "False" project = 'CompLib' copyright = '2022, Verein zur Förderung von Wissenschaft und Technik an Schulen (F-WuTS)' author = 'robo4you' +autoclass_content = 'both' # The full version, including alpha/beta/rc tags release = '0.2.3' @@ -59,5 +60,10 @@ html_theme = 'sphinx_rtd_theme' # so a file named "default.css" will overwrite the builtin "default.css". html_static_path = ['_static'] +html_logo = "images/compair-logo-white.svg" +html_theme_options = { + 'logo_only': True, + 'display_version': False, +} language = "de" diff --git a/docs/source/faq.rst b/docs/source/faq.rst new file mode 100644 index 0000000..2e2f4b3 --- /dev/null +++ b/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 `_ + +Software zum Schreiben der SD-Karte `balenaEtcher `_ + + diff --git a/client/docs/source/gettingStarted/codeServer.rst b/docs/source/gettingStarted/codeServer.rst similarity index 100% rename from client/docs/source/gettingStarted/codeServer.rst rename to docs/source/gettingStarted/codeServer.rst diff --git a/client/docs/source/gettingStarted/firstProgram.rst b/docs/source/gettingStarted/firstProgram.rst similarity index 100% rename from client/docs/source/gettingStarted/firstProgram.rst rename to docs/source/gettingStarted/firstProgram.rst diff --git a/client/docs/source/gettingStarted/images/01_boot.png b/docs/source/gettingStarted/images/01_boot.png similarity index 100% rename from client/docs/source/gettingStarted/images/01_boot.png rename to docs/source/gettingStarted/images/01_boot.png diff --git a/client/docs/source/gettingStarted/images/02_psk.png b/docs/source/gettingStarted/images/02_psk.png similarity index 100% rename from client/docs/source/gettingStarted/images/02_psk.png rename to docs/source/gettingStarted/images/02_psk.png diff --git a/client/docs/source/gettingStarted/images/03_codeServerFile.png b/docs/source/gettingStarted/images/03_codeServerFile.png similarity index 100% rename from client/docs/source/gettingStarted/images/03_codeServerFile.png rename to docs/source/gettingStarted/images/03_codeServerFile.png diff --git a/client/docs/source/gettingStarted/images/04_codeServerRun.png b/docs/source/gettingStarted/images/04_codeServerRun.png similarity index 100% rename from client/docs/source/gettingStarted/images/04_codeServerRun.png rename to docs/source/gettingStarted/images/04_codeServerRun.png diff --git a/client/docs/source/gettingStarted/images/05_codeServerTerminal.png b/docs/source/gettingStarted/images/05_codeServerTerminal.png similarity index 100% rename from client/docs/source/gettingStarted/images/05_codeServerTerminal.png rename to docs/source/gettingStarted/images/05_codeServerTerminal.png diff --git a/client/docs/source/gettingStarted/images/06_codeServerFolder.png b/docs/source/gettingStarted/images/06_codeServerFolder.png similarity index 100% rename from client/docs/source/gettingStarted/images/06_codeServerFolder.png rename to docs/source/gettingStarted/images/06_codeServerFolder.png diff --git a/client/docs/source/gettingStarted/images/07_irSensor.webp b/docs/source/gettingStarted/images/07_irSensor.webp similarity index 100% rename from client/docs/source/gettingStarted/images/07_irSensor.webp rename to docs/source/gettingStarted/images/07_irSensor.webp diff --git a/docs/source/gettingStarted/images/08_notepad.png b/docs/source/gettingStarted/images/08_notepad.png new file mode 100644 index 0000000..c2ac6c5 Binary files /dev/null and b/docs/source/gettingStarted/images/08_notepad.png differ diff --git a/docs/source/gettingStarted/images/09_update.png b/docs/source/gettingStarted/images/09_update.png new file mode 100644 index 0000000..bd73d23 Binary files /dev/null and b/docs/source/gettingStarted/images/09_update.png differ diff --git a/client/docs/source/gettingStarted/index.rst b/docs/source/gettingStarted/index.rst similarity index 91% rename from client/docs/source/gettingStarted/index.rst rename to docs/source/gettingStarted/index.rst index d3333ef..8f0a94b 100644 --- a/client/docs/source/gettingStarted/index.rst +++ b/docs/source/gettingStarted/index.rst @@ -7,5 +7,6 @@ Erste Schritte wifi.rst codeServer.rst firstProgram.rst + update.rst secondProgram.rst thridProgram.rst diff --git a/client/docs/source/gettingStarted/secondProgram.rst b/docs/source/gettingStarted/secondProgram.rst similarity index 100% rename from client/docs/source/gettingStarted/secondProgram.rst rename to docs/source/gettingStarted/secondProgram.rst diff --git a/client/docs/source/gettingStarted/thridProgram.rst b/docs/source/gettingStarted/thridProgram.rst similarity index 100% rename from client/docs/source/gettingStarted/thridProgram.rst rename to docs/source/gettingStarted/thridProgram.rst diff --git a/docs/source/gettingStarted/update.rst b/docs/source/gettingStarted/update.rst new file mode 100644 index 0000000..f09fae9 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/client/docs/source/gettingStarted/wifi.rst b/docs/source/gettingStarted/wifi.rst similarity index 87% rename from client/docs/source/gettingStarted/wifi.rst rename to docs/source/gettingStarted/wifi.rst index f845842..343ba26 100644 --- a/client/docs/source/gettingStarted/wifi.rst +++ b/docs/source/gettingStarted/wifi.rst @@ -33,6 +33,15 @@ Weitere Informationen Die "wpa_supplicant.conf" Datei wird beim Start des Rpasberry Pi automatisch an den richtigen Ort kopiert, damit sich der Roboter zum Wlan verbindet. Eine genauere Anleitung wird vom Hersteller des Raspberry Pi `hier `_ bereitgestellt. +Windows...... +------------- +Je nach Betriebssystem und Editor, mit dem Sie die Datei erstellen, könnte die Datei falsche Zeilenumbrüche oder eine falsche Dateierweiterung haben; stellen Sie also sicher, dass Sie einen Editor verwenden, der dies berücksichtigt. Linux erwartet das Zeilenumbruchzeichen LF (Line Feed). +Beispielsweise kann `Notepad++ `_ verwendet werden, um die Datei richtig zu speichern. +|notepadImage| + +.. |notepadImage| image:: images/08_notepad.png + + Fehlerbehandlung ---------------- Sollte es dazu kommen, dass der Roboter nicht automatisch die Verbindung mit dem Netzwerk herstellt, kann eine Kabelgebundene Verbindung zur Diagnose von Fehlern genutzt werden. diff --git a/client/docs/source/images/compair-logo-white.svg b/docs/source/images/compair-logo-white.svg similarity index 100% rename from client/docs/source/images/compair-logo-white.svg rename to docs/source/images/compair-logo-white.svg diff --git a/client/docs/source/index.rst b/docs/source/index.rst similarity index 80% rename from client/docs/source/index.rst rename to docs/source/index.rst index 840e6d7..7d70efd 100644 --- a/client/docs/source/index.rst +++ b/docs/source/index.rst @@ -1,5 +1,3 @@ -.. image:: images/compair-logo-white.svg - Dokumentation des Roboters ########################## @@ -7,8 +5,8 @@ Dokumentation des Roboters :maxdepth: 2 :caption: Contents: -Contents -********* +Inhalt +****** .. toctree:: :maxdepth: 1 @@ -19,3 +17,4 @@ Contents faq.rst other/usage lib/index.rst + other/hardware.rst diff --git a/docs/source/lib/classes/DoubleElimination.rst b/docs/source/lib/classes/DoubleElimination.rst new file mode 100644 index 0000000..240f713 --- /dev/null +++ b/docs/source/lib/classes/DoubleElimination.rst @@ -0,0 +1,13 @@ +.. _lib_doubleElim: + +Double Elimination +******************* + +Dokumentation des Double Elimination Moduls +============================================ + +.. autoclass:: compLib.DoubleElimination.Position + :members: + +.. autoclass:: compLib.DoubleElimination.DoubleElim + :members: diff --git a/docs/source/lib/classes/Encoder.rst b/docs/source/lib/classes/Encoder.rst new file mode 100644 index 0000000..ecca45c --- /dev/null +++ b/docs/source/lib/classes/Encoder.rst @@ -0,0 +1,10 @@ +.. _lib_encoder: + +Encoder +******* + +Dokumentation der Klasse +======================== + +.. autoclass:: compLib.Encoder.Encoder + :members: diff --git a/docs/source/lib/classes/IRSensor.rst b/docs/source/lib/classes/IRSensor.rst new file mode 100644 index 0000000..1454156 --- /dev/null +++ b/docs/source/lib/classes/IRSensor.rst @@ -0,0 +1,10 @@ +.. _lib_irsensor: + +Infrarot Sensoren +***************** + +Dokumentation der Klasse +======================== + +.. autoclass:: compLib.IRSensor.IRSensor + :members: diff --git a/client/docs/source/lib/classes/Motor.rst b/docs/source/lib/classes/Motor.rst similarity index 100% rename from client/docs/source/lib/classes/Motor.rst rename to docs/source/lib/classes/Motor.rst diff --git a/docs/source/lib/classes/Opencv.rst b/docs/source/lib/classes/Opencv.rst new file mode 100644 index 0000000..9d788db --- /dev/null +++ b/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. diff --git a/docs/source/lib/classes/Seeding.rst b/docs/source/lib/classes/Seeding.rst new file mode 100644 index 0000000..e56de99 --- /dev/null +++ b/docs/source/lib/classes/Seeding.rst @@ -0,0 +1,36 @@ +.. _lib_seeding: + +Seeding +******* + +Dokumentation des Seeding Moduls +================================ + +.. autoclass:: compLib.Seeding.Gamestate + :members: + +Beispiele +---------- + +| In ``Zeile 1`` wird das Seeding Modul importiert. +| In ``Zeile 2`` definieren wir dann eine Variable, in der wir den "Seed" des Gamestates den wir erstellen wollten speichern. +| In ``Zeile 3`` erstellen wir dann einen neuen Gamestate mit dem Seed und speichern ihn in die Variable ``gamestate``. +| In ``Zeile 4`` geben wir dann den Gamestate aus, damit wir ihn auf der Konsole ansehen können. + +.. code-block:: python + + import compLib.Seeding as Seeding + seed = 42 + gamestate = Seeding.Gamestate(seed) + print(gamestate) + +In der Ausgabe des Print Statements sehen wir den generierten Gamestate. + +.. code-block:: + + Seed: 42 + Heu Color: 1 + Material Pairs: [[3, 0], [2, 3], [0, 2], [1, 2]] + Material Zones: [2, 1, 3, 2] + Logistic Plan: [12, 13, 12, 13, 10, 11, 13, 10, 13, 12, 11, 10, 11, 13, 10, 11, 12, 11, 12, 10, 12] + Logistic Centers: [[0, 3, 1, 1], [1, 0, 2, 2], [1, 2, 0, 2], [3, 0, 2, 0]] diff --git a/docs/source/lib/classes/images/6x6_1000-0.png b/docs/source/lib/classes/images/6x6_1000-0.png new file mode 100644 index 0000000..747e1e1 Binary files /dev/null and b/docs/source/lib/classes/images/6x6_1000-0.png differ diff --git a/client/docs/source/lib/classes/images/chessboard.jpg b/docs/source/lib/classes/images/chessboard.jpg similarity index 100% rename from client/docs/source/lib/classes/images/chessboard.jpg rename to docs/source/lib/classes/images/chessboard.jpg diff --git a/client/docs/source/lib/classes/images/chessboard_detected.jpg b/docs/source/lib/classes/images/chessboard_detected.jpg similarity index 100% rename from client/docs/source/lib/classes/images/chessboard_detected.jpg rename to docs/source/lib/classes/images/chessboard_detected.jpg diff --git a/client/docs/source/lib/classes/images/opencv_http_stream.png b/docs/source/lib/classes/images/opencv_http_stream.png similarity index 100% rename from client/docs/source/lib/classes/images/opencv_http_stream.png rename to docs/source/lib/classes/images/opencv_http_stream.png diff --git a/client/docs/source/lib/classes/images/opencv_processed.png b/docs/source/lib/classes/images/opencv_processed.png similarity index 100% rename from client/docs/source/lib/classes/images/opencv_processed.png rename to docs/source/lib/classes/images/opencv_processed.png diff --git a/docs/source/lib/index.rst b/docs/source/lib/index.rst new file mode 100644 index 0000000..d781e5e --- /dev/null +++ b/docs/source/lib/index.rst @@ -0,0 +1,12 @@ +compLib +####### + +.. toctree:: + :maxdepth: 5 + + classes/Motor + classes/Encoder + classes/IRSensor + classes/Seeding + classes/DoubleElimination + classes/Opencv diff --git a/docs/source/other/hardware.rst b/docs/source/other/hardware.rst new file mode 100644 index 0000000..81b96c1 --- /dev/null +++ b/docs/source/other/hardware.rst @@ -0,0 +1,64 @@ +.. _other_bardware: + +Hardware +######## + +Sensorarray +*********** + +|SensorarrayImage| + +.. |SensorarrayImage| image:: images/Sensorarray.png + +Specs V4 +-------- + +| **Processor:** `STM32G030F6P6 `_ - 32-bit ARM Cortex M0 CPU @ 64 MHz +| **I/O:** 1x I2C, 1x SWD +| **Sensors:** 5x `QRE1113GR `_ + +Specs V2 +-------- + +| **Processor:** `ATMEGA328P-AU `_ - 8-bit CPU @ 16 MHz +| **I/O:** 1x I2C, 1x UART, 1x ISP +| **Sensors:** 5x `QRE1113GR `_ + +Details +------- + +Das Sensorarray wird verwendet um Linienen vor dem Roboter zu erkennen. Es agiert als I2C Slave und muss dementsprechend aktiv gepollt werden. +Zusätzlich besteht die möglichkeit alle Emitter zu deaktiviern um einen eventuellen Messfehler durch Sonneneinstralung oder andere Störquellen zu erkennen. + +Version 4 unterscheidet sich zu Version 2 im Mikroprozessor, da es zu Lieferengpässen des ATMEGA gekommen ist. +Zusätzlich wurde die möglichkeit alle Emitter einzeln an bzw. auszuschalten entfernt, da diese keinen signifikanten Mehrwert brachte. + +Motorboard +********** + +|MainboardImage| + +.. |MainboardImage| image:: images/Mainboard.png + +Specs +----- +**Motor-Treiber:** `LV8548MC-AH `_ + +Details +------- +Das Motorboard kann an einen der 4 Ports am Roboter angesteckt werden und ermöglicht das Ansteuern von Motoren und auslesen von Encodern. + +Mainboard +********* + +Specs +----- + +| **Processor:** `STM32L051C8T6TR `_ - 32-bit ARM Cortex M0 @ 32MHz +| **I/O:** 4x I2C (3x Bus 1, 1x Bus 2), 1x 40 Pin GPIO Header, 2x SPI (Verbunden mit GPIO), 4x Motor-/Servo-connector, 1x SWD, 1x USB-C + +Details +------- + +Das Mainboard wird auf den GPIO-Header eines Raspberry Pi gesteckt und ermöglicht die Steuerung eines Roboters mittels 4 Motor- bzw. Servo-Ports. Der RaspberryPi kommuniziert dabei mittels SPI mit dem Mainboard und steuert die einzelnen Sensoren oder Module an. +Zusätzlich befinden sich auf der Unterseite des Mainboards Lötstellen, welche direkt mit der Stromversorgung der Motoren verbunden sind und geben so die möglichkeit Motoren mit mehr als 5V anzusteuern. \ No newline at end of file diff --git a/docs/source/other/images/Mainboard.png b/docs/source/other/images/Mainboard.png new file mode 100644 index 0000000..afe6987 Binary files /dev/null and b/docs/source/other/images/Mainboard.png differ diff --git a/docs/source/other/images/Sensorarray.png b/docs/source/other/images/Sensorarray.png new file mode 100644 index 0000000..11c4128 Binary files /dev/null and b/docs/source/other/images/Sensorarray.png differ diff --git a/docs/source/other/usage.rst b/docs/source/other/usage.rst new file mode 100644 index 0000000..97f67d4 --- /dev/null +++ b/docs/source/other/usage.rst @@ -0,0 +1,166 @@ +.. _other_usage: + +Beispiele +######### + +Vorwärts und rückwärts fahren +***************************** + +.. code-block:: python + + import time + from compLib.Motor import * + + def forward(): + Motor.power(0, -30) + Motor.power(3, 30) + + + def backward(): + Motor.power(0, 30) + Motor.power(3, -30) + + def main(): + print("hallo ich bin ein roboter beep buup") + + forward() + time.sleep(1) + backward() + time.sleep(1) + + if __name__ == '__main__': + main() + + +Eine Linie verfolgen +******************** + +.. code-block:: python + + import time + from compLib.Motor import Motor + from compLib.Encoder import Encoder + from compLib.IRSensor import IRSensor + + COLOR_BREAK = 850 + DRIVE_SPEED = 35 + + IRSensor.enable() + + def drive(left, right): + right *= -1 + Motor.multiple_power((0, right), (3, left)) + print(f"{left} {right}") + + def follow(): + while True: + sensors = IRSensor.read_all() + + if sensors[0] > COLOR_BREAK: + # turn left + drive(-DRIVE_SPEED, DRIVE_SPEED) + elif sensors[4] > COLOR_BREAK: + # turn right + drive(DRIVE_SPEED, -DRIVE_SPEED) + else: + # straight + drive(DRIVE_SPEED, DRIVE_SPEED) + + if sensors[0] > COLOR_BREAK and sensors[4] > COLOR_BREAK: + break + + drive(0, 0) + time.sleep(1) + + def main(): + follow() + + drive(DRIVE_SPEED, DRIVE_SPEED) + time.sleep(0.5) + follow() + + drive(DRIVE_SPEED, DRIVE_SPEED) + time.sleep(0.5) + follow() + + drive(DRIVE_SPEED, DRIVE_SPEED) + time.sleep(0.5) + follow() + + if __name__ == "__main__": + main() + +Funktionalität des Roboters überprüfen +************************************** + +.. code-block:: python + + import time + from compLib.Motor import Motor + from compLib.Encoder import Encoder + from compLib.IRSensor import IRSensor + + + def testIR(): + print("Enabling Infrared Sensor") + IRSensor.enable() + time.sleep(1) + + print("Writing sensor values...") + for i in range(0, 50): + print(IRSensor.read_all()) + time.sleep(0.1) + + print("Disabling Infrared Sensor") + IRSensor.disable() + + def testEncoders(): + Motor.multiple_pulse_width((0, 50), (3, -50)) + + print("Writing encoder positions...") + for i in range(0, 50): + print(Encoder.read_all_positions()) + time.sleep(0.1) + + time.sleep(2) + print("Writing encoder velocities...") + for i in range(0, 50): + print(Encoder.read_all_velocities()) + time.sleep(0.1) + + Motor.multiple_pulse_width((0, 0), (3, 0)) + + + def testMotors(): + print("Setting pulse_with") + Motor.multiple_pulse_width((0, 50), (3, -50)) + time.sleep(3) + + print("Setting power") + Motor.multiple_power((0, 50), (3, -50)) + time.sleep(3) + + print("Setting pulse_with") + Motor.multiple_speed((0, 5), (3, -5)) + time.sleep(3) + + for i in range(0, 100): + Motor.multiple_power((0, i), (3, -i)) + time.sleep(0.1) + + + if __name__ == "__main__": + print("Make sure robot is turned on it's back!") + time.sleep(5) + + print() + print("----------------- Testing Infrared Sensor -----------------") + testIR() + + print() + print("----------------- Testing Encoder -----------------") + testEncoders() + + print() + print("----------------- Testing Motors -----------------") + testMotors() \ No newline at end of file diff --git a/docs/source/software/installation.rst b/docs/source/software/installation.rst new file mode 100644 index 0000000..1a648fe --- /dev/null +++ b/docs/source/software/installation.rst @@ -0,0 +1,59 @@ +.. _software_installation: + +Installationsanweisungen +######################## + +Diese Anleitung dient dazu die Software auf dem Roboter neu aufzusetzen. + +**Im normalen Gebraucht sollte dies jedoch nicht notwendig sein.** + +Betriebssystem aufsetzen +======================== + +Als Basis wird für den Roboter Raspberry Pi OS (64-bit) verwendet. Das 32-Bit Betriebssystem wird nicht unterstützt, da die Software-Komponenten nur für aarch64 bzw. arm64/v8 kompiliert werden. +Genauere Informationen sind `hier `_ zu finden. + +Bearbeiten der boot-Partition +============================= + +1. ``cmdline.txt`` + +:: + + console=tty1 root=PARTUUID=21e60f8c-02 rootfstype=ext4 fsck.repair=yes rootwait quiet init=/usr/lib/raspi-config/init_resize.sh + + +Stellen Sie sicher, dass die folgenden Einstellungen in der ``config.txt`` korrekt gesetzt sind + +2. ``config.txt`` + +:: + + # SPI + dtparam=spi=on + dtoverlay=spi1-3cs + + # Run in 64-bit mode + arm_64bit=1 + + [all] + + [pi4] + # Run as fast as firmware / board allows + arm_boost=1 + + [all] + start_x=1 + gpu_mem=128 + + dtoverlay=pi3-disable-bt + enable_uart=1 + +3. Erstellen der leeren Datei ``ssh``, damit ssh beim nächsten Start aktiviert wird +4. Hinzufügen der ``userconf.txt`` + +:: + + compair:$6$eh2pkHm18RgYtwiG$PoeabVCH8llbyIio66OefPGXZ2r2BRI2hPHIdkNTBjmiR0lGXsozGyLx0uViOx3bi998syXjSDXkwt0t3x8Bq. + +5. Wlan Verbindung einrichten \ No newline at end of file diff --git a/client/motorLinearityTest.py b/motorLinearityTest.py similarity index 100% rename from client/motorLinearityTest.py rename to motorLinearityTest.py diff --git a/client/postinstall.sh b/postinstall.sh old mode 100644 new mode 100755 similarity index 82% rename from client/postinstall.sh rename to postinstall.sh index 2612645..b37be50 --- a/client/postinstall.sh +++ b/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 diff --git a/client/setup.py b/setup.py similarity index 92% rename from client/setup.py rename to setup.py index fb84e02..8fffc86 100644 --- a/client/setup.py +++ b/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="", diff --git a/client/sphinx_to_github.sh b/sphinx_to_github.sh similarity index 100% rename from client/sphinx_to_github.sh rename to sphinx_to_github.sh diff --git a/client/test.py b/test.py similarity index 56% rename from client/test.py rename to test.py index 0f54f54..4da0034 100644 --- a/client/test.py +++ b/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()