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

.idea/.gitignore

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

.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>

.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>

.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>

.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>

.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>

.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>

.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>

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
@@ -43,4 +47,4 @@ ffmpeg -f v4l2  -framerate 30 -video_size 640x480 -i /dev/video0 -b:v 2M -f flv
 
 https://www.raspberrypi.com/news/bullseye-camera-system/
 
-(This can still be mitigated by enabling "old camera support" in der raspi-config settings. (This is eine automatically in der postinstallscript)
+(This can still be mitigated by enabling "old camera support" in the raspi-config settings. (This is done automatically in the postinstallscript)

build.sh

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

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,5 +1,5 @@
 import sys
-from typing import Any
+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")
@@ -58,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(),
@@ -67,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")
@@ -121,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)
@@ -154,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.
@@ -167,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
         """
@@ -177,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/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/lib/classes/Opencv.rst

@@ -11,3 +11,69 @@ Dokumentation des Camera Moduls
 
 .. 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.

postinstall.sh

@@ -1,6 +1,6 @@
 #!/usr/bin/env bash
 
-if [[ $(uname -m) == "x86_64" ]]; then
+if [ "$(uname -m)" = "x86_64" ]; then
   echo "Not running on RPi - Skipping postinstall"
   exit 0
 fi