From b95b9bf5187da8c07ef1e95b97899ac74a969be2 Mon Sep 17 00:00:00 2001 From: itssme Date: Sun, 18 Dec 2022 14:00:11 +0100 Subject: [PATCH 01/11] (MINOR) updated readme --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 79b1f1f..381e0fb 100644 --- a/README.md +++ b/README.md @@ -42,3 +42,5 @@ ffmpeg -f v4l2 -framerate 30 -video_size 640x480 -i /dev/video0 -b:v 2M -f flv # Bullseye now only supports libcamera https://www.raspberrypi.com/news/bullseye-camera-system/ + +(This can still be mitigated by enabling "old camera support" in der raspi-config settings. (This is eine automatically in der postinstallscript) From 671292a6cf9a0874e8fa749b880bda4aadc56942 Mon Sep 17 00:00:00 2001 From: Joel Klimont Date: Sun, 18 Dec 2022 15:08:20 +0100 Subject: [PATCH 02/11] fixed moving built package to the correct output directory --- README.md | 2 +- build.sh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 381e0fb..0c64463 100644 --- a/README.md +++ b/README.md @@ -43,4 +43,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) diff --git a/build.sh b/build.sh index f9f3020..670cd27 100644 --- a/build.sh +++ b/build.sh @@ -6,4 +6,4 @@ DEB="empty" source build_deb.sh echo "Ran build deb, created: $DEB" -mv $DEB ../output +mv $DEB ./output From f09d20ade2ff1db11ea23ca425a9858d48674489 Mon Sep 17 00:00:00 2001 From: Joel Klimont Date: Sat, 14 Jan 2023 15:11:57 +0100 Subject: [PATCH 03/11] removed .idea folder from repo --- .idea/.gitignore | 3 --- .idea/client_s2.iml | 8 -------- .idea/inspectionProfiles/Project_Default.xml | 12 ------------ .idea/inspectionProfiles/profiles_settings.xml | 6 ------ .idea/misc.xml | 7 ------- .idea/modules.xml | 8 -------- .idea/saveactions_settings.xml | 15 --------------- .idea/vcs.xml | 6 ------ 8 files changed, 65 deletions(-) delete mode 100644 .idea/.gitignore delete mode 100644 .idea/client_s2.iml delete mode 100644 .idea/inspectionProfiles/Project_Default.xml delete mode 100644 .idea/inspectionProfiles/profiles_settings.xml delete mode 100644 .idea/misc.xml delete mode 100644 .idea/modules.xml delete mode 100644 .idea/saveactions_settings.xml delete mode 100644 .idea/vcs.xml diff --git a/.idea/.gitignore b/.idea/.gitignore deleted file mode 100644 index 26d3352..0000000 --- a/.idea/.gitignore +++ /dev/null @@ -1,3 +0,0 @@ -# Default ignored files -/shelf/ -/workspace.xml diff --git a/.idea/client_s2.iml b/.idea/client_s2.iml deleted file mode 100644 index d0876a7..0000000 --- a/.idea/client_s2.iml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml deleted file mode 100644 index 9277599..0000000 --- a/.idea/inspectionProfiles/Project_Default.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml deleted file mode 100644 index 105ce2d..0000000 --- a/.idea/inspectionProfiles/profiles_settings.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml deleted file mode 100644 index f6104af..0000000 --- a/.idea/misc.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml deleted file mode 100644 index 8417644..0000000 --- a/.idea/modules.xml +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/.idea/saveactions_settings.xml b/.idea/saveactions_settings.xml deleted file mode 100644 index b7b3f34..0000000 --- a/.idea/saveactions_settings.xml +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml deleted file mode 100644 index 6c0b863..0000000 --- a/.idea/vcs.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file From cf415d38e812b859608fea11cfd7ee0a1bd711c1 Mon Sep 17 00:00:00 2001 From: Joel Klimont Date: Fri, 10 Feb 2023 13:44:28 +0100 Subject: [PATCH 04/11] fixed documentation --- compLib/Camera.py | 24 +++++-- docs/source/lib/classes/Opencv.rst | 66 ++++++++++++++++++ docs/source/lib/classes/images/6x6_1000-0.png | Bin 0 -> 5880 bytes 3 files changed, 83 insertions(+), 7 deletions(-) create mode 100644 docs/source/lib/classes/images/6x6_1000-0.png diff --git a/compLib/Camera.py b/compLib/Camera.py index 3a7d764..0937ec1 100644 --- a/compLib/Camera.py +++ b/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: diff --git a/docs/source/lib/classes/Opencv.rst b/docs/source/lib/classes/Opencv.rst index bd299c1..9d788db 100644 --- a/docs/source/lib/classes/Opencv.rst +++ b/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. 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 0000000000000000000000000000000000000000..747e1e152b9c3249baab59c86fcff38544fb8b53 GIT binary patch literal 5880 zcmeAS@N?(olHy`uVBq!ia0y~yV15C@9Be=l-^Ev+FfcH-W;#0ucse^P6cpvBW#*(Z zFlbDyooMTE*g@uKy!{d-Q9WnP5;|!sjdqR}- zQZ{Kjy%2TeSs<NcK25tGS$!5xIwDaKd5K{rA&)#Ctyq*b z<^A~`YYOf?S~E>+O`-q4Lj``(agVIdI;w1FaP1IKSlq-rB|*_`wOd!G$lK$#fByEx zt4#M<$7@h@W-*WGq{r<4oA2M9^|Q0kTvLYqR$R{SWDbtEOkY>!*2~{6`^&;%!yF!U zIes1E<@rm(XGeYEcyWtOZ{6%!d)Q;%G1U6+O7Z9uS@81w^TzMSzn1A-DCRC%vum&E zx58fxOIEIa!dpH=O_NOe;g4*0H&AT%h7(~8!x;TbZ+ESN1f0LY+98=99G;_spag~|K%!-Z-(0@NaazTYsp9#MdCVV*^g!k0XgGo^4yvW0 z(RzoRiulH>84T~PgHvZyIUhq!5BF&81I(PbD;h{LBe6OrzsMcQRoJ#uXRh!RY`q7|lAv)35-V;3-Eb Date: Fri, 10 Feb 2023 14:02:07 +0100 Subject: [PATCH 05/11] fixed syntax error in postinstallscript --- postinstall.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/postinstall.sh b/postinstall.sh index f2a5b39..4f6f94e 100755 --- a/postinstall.sh +++ b/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 From fa4cc1a0ad7a7ecb6d2a06599c5e7a41c7ca17c0 Mon Sep 17 00:00:00 2001 From: Joel Klimont Date: Fri, 10 Feb 2023 14:29:13 +0100 Subject: [PATCH 06/11] fixed syntax error in postinstallscript --- postinstall.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/postinstall.sh b/postinstall.sh index 4f6f94e..b37be50 100755 --- a/postinstall.sh +++ b/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 From de112d98e423d3d4d45a040dbbae73724e78dbf5 Mon Sep 17 00:00:00 2001 From: Matthias Guzmits Date: Tue, 18 Jul 2023 00:44:34 +0200 Subject: [PATCH 07/11] Added basic drive functions --- compLib/CMakeLists.txt | 55 ++++++++++ compLib/include/comp_lib_node.h | 14 +++ compLib/include/motor.h | 54 ++++++++++ compLib/include/sequence_lock.h | 11 ++ compLib/package.xml | 22 ++++ compLib/src/comp_lib_node.cpp | 10 ++ compLib/src/main.cpp | 66 ++++++++++++ compLib/src/motor.cpp | 171 ++++++++++++++++++++++++++++++++ 8 files changed, 403 insertions(+) create mode 100644 compLib/CMakeLists.txt create mode 100644 compLib/include/comp_lib_node.h create mode 100644 compLib/include/motor.h create mode 100644 compLib/include/sequence_lock.h create mode 100644 compLib/package.xml create mode 100644 compLib/src/comp_lib_node.cpp create mode 100644 compLib/src/main.cpp create mode 100644 compLib/src/motor.cpp diff --git a/compLib/CMakeLists.txt b/compLib/CMakeLists.txt new file mode 100644 index 0000000..113ea36 --- /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) +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/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/motor.h b/compLib/include/motor.h new file mode 100644 index 0000000..24e6ce4 --- /dev/null +++ b/compLib/include/motor.h @@ -0,0 +1,54 @@ +#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/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; +}; + +#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/main.cpp b/compLib/src/main.cpp new file mode 100644 index 0000000..87ba18b --- /dev/null +++ b/compLib/src/main.cpp @@ -0,0 +1,66 @@ +#include +#include +#include +#include + +#include "motor.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); +} + +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(); + + std::thread t; + std::thread t1; + std::thread t2; + + t = std::thread(run_node, ddn); + t1 = std::thread(run_node1, ssn); + t2 = std::thread(run_node2, ran); + + ssn->drive(0.2); + + 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)); + + ddn->kill(); + ssn->kill(); + ddn->kill(); + + t.join(); + t1.join(); + t2.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..8adef43 --- /dev/null +++ b/compLib/src/motor.cpp @@ -0,0 +1,171 @@ +#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/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) +{ + std::cout << "drive dist" << std::endl; + 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; + std::cout << "finished dist" << std::endl; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + 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() +{ + std::cout << "DriveDistNode killed" << std::endl; + rclcpp::shutdown(); +} + +SetSpeedNode::SetSpeedNode() +: Node("set_speed_node") +{ + speed_publisher_ = this->create_publisher( + "/cmd_vel", + rclcpp::SensorDataQoS() + ); +} + +void SetSpeedNode::drive_loop(float speed) +{ + std::cout << "Loop" << std::endl; + 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) +{ + std::cout << "Drive" << std::endl; + run = true; + t = std::thread(&SetSpeedNode::drive_loop, this, speed); +} + +void SetSpeedNode::stop() +{ + std::cout << "stop" << std::endl; + run = false; +} + +void SetSpeedNode::kill() +{ + std::cout << "SetSpeedNode killed" << std::endl; + 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; + std::cout << "finished rotation" << std::endl; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + 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() +{ + std::cout << "RotateAngleNode killed" << std::endl; + rclcpp::shutdown(); +} \ No newline at end of file From 9c40daae888ae4fee92dededd3049c22366379be Mon Sep 17 00:00:00 2001 From: itssme Date: Tue, 18 Jul 2023 09:17:25 +0200 Subject: [PATCH 08/11] (MAJOR) Bump to new major version --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 0c64463..78d7204 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # compLIB +Rewrite for ROS is the live packaged version since 18.07.2023. + # Dependencies ## Building documentation From 27962d16a79f508bd8890175c18a2c7046d40ce6 Mon Sep 17 00:00:00 2001 From: itssme Date: Tue, 18 Jul 2023 09:18:33 +0200 Subject: [PATCH 09/11] (MAJOR) Bump to new major version after deleting invalid tag --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 78d7204..33f9085 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,8 @@ Rewrite for ROS is the live packaged version since 18.07.2023. # Dependencies +TODO: document + ## Building documentation ``` pip install sphinx-rtd-theme From ed04957d94e0067e2e5e58d2b4ec3c9752ce7421 Mon Sep 17 00:00:00 2001 From: Matthias Guzmits Date: Wed, 19 Jul 2023 18:36:07 +0200 Subject: [PATCH 10/11] Add drive arc functionality --- compLib/include/motor.h | 13 ++++++++ compLib/src/main.cpp | 14 +++++++- compLib/src/motor.cpp | 72 +++++++++++++++++++++++++++++++++++------ 3 files changed, 89 insertions(+), 10 deletions(-) diff --git a/compLib/include/motor.h b/compLib/include/motor.h index 24e6ce4..a2096c0 100644 --- a/compLib/include/motor.h +++ b/compLib/include/motor.h @@ -10,6 +10,7 @@ #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 @@ -51,4 +52,16 @@ private: 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/src/main.cpp b/compLib/src/main.cpp index 87ba18b..c10699b 100644 --- a/compLib/src/main.cpp +++ b/compLib/src/main.cpp @@ -23,6 +23,11 @@ void run_node2(std::shared_ptr node) rclcpp::spin(node); } +void run_node3(std::shared_ptr node) +{ + rclcpp::spin(node); +} + int main(int argc, char * argv[]) { rclcpp::init(argc, argv); @@ -30,16 +35,19 @@ int main(int argc, char * argv[]) auto ddn = std::make_shared(); auto ssn = std::make_shared(); auto ran = std::make_shared(); + auto dan = std::make_shared(); std::thread t; std::thread t1; std::thread t2; + std::thread t3; t = std::thread(run_node, ddn); t1 = std::thread(run_node1, ssn); t2 = std::thread(run_node2, ran); + t3 = std::thread(run_node3, dan); - ssn->drive(0.2); + ssn->drive(0.3); std::this_thread::sleep_for (std::chrono::milliseconds(2000)); @@ -54,13 +62,17 @@ int main(int argc, char * argv[]) // 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(); t.join(); t1.join(); t2.join(); + t3.join(); return 0; } \ No newline at end of file diff --git a/compLib/src/motor.cpp b/compLib/src/motor.cpp index 8adef43..a0e4528 100644 --- a/compLib/src/motor.cpp +++ b/compLib/src/motor.cpp @@ -10,6 +10,7 @@ #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" @@ -28,7 +29,7 @@ DriveDistNode::DriveDistNode() void DriveDistNode::drive_dist(float meters, float velocity) { - std::cout << "drive dist" << std::endl; + 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(); @@ -45,9 +46,9 @@ void DriveDistNode::drive_dist(float meters, float velocity) void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) { processing = false; - std::cout << "finished dist" << std::endl; 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"); @@ -63,7 +64,7 @@ void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandleget_logger(), "DriveDistNode killed"); rclcpp::shutdown(); } @@ -78,7 +79,6 @@ SetSpeedNode::SetSpeedNode() void SetSpeedNode::drive_loop(float speed) { - std::cout << "Loop" << std::endl; while (run) { set_speed(speed); @@ -102,20 +102,20 @@ void SetSpeedNode::set_speed(float speed) void SetSpeedNode::drive(float speed) { - std::cout << "Drive" << std::endl; + RCLCPP_INFO(this->get_logger(), "Start drive"); run = true; t = std::thread(&SetSpeedNode::drive_loop, this, speed); } void SetSpeedNode::stop() { - std::cout << "stop" << std::endl; run = false; + RCLCPP_INFO(this->get_logger(), "Stop drive"); } void SetSpeedNode::kill() { - std::cout << "SetSpeedNode killed" << std::endl; + RCLCPP_INFO(this->get_logger(), "SetSpeedNode killed"); rclcpp::shutdown(); } @@ -148,9 +148,9 @@ void RotateAngleNode::rotate_angle(float angle, float velocity) void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult & result) { processing = false; - std::cout << "finished rotation" << std::endl; 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"); @@ -166,6 +166,60 @@ void RotateAngleNode::result_callback(const rclcpp_action::ClientGoalHandleget_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 From c593059fb15624e2e58a9effc606f4a9f4eb5d79 Mon Sep 17 00:00:00 2001 From: Matthias Guzmits Date: Wed, 19 Jul 2023 19:17:59 +0200 Subject: [PATCH 11/11] Add button functionality --- compLib/CMakeLists.txt | 2 +- compLib/include/controls.h | 24 +++++++++++++++++ compLib/src/controls.cpp | 53 ++++++++++++++++++++++++++++++++++++++ compLib/src/main.cpp | 15 +++++++++++ 4 files changed, 93 insertions(+), 1 deletion(-) create mode 100644 compLib/include/controls.h create mode 100644 compLib/src/controls.cpp diff --git a/compLib/CMakeLists.txt b/compLib/CMakeLists.txt index 113ea36..d539099 100644 --- a/compLib/CMakeLists.txt +++ b/compLib/CMakeLists.txt @@ -25,7 +25,7 @@ find_package(geometry_msgs REQUIRED) # 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) +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) 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/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 index c10699b..5c6296f 100644 --- a/compLib/src/main.cpp +++ b/compLib/src/main.cpp @@ -4,6 +4,7 @@ #include #include "motor.h" +#include "controls.h" std::mutex action_mutex; @@ -28,6 +29,11 @@ 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); @@ -36,16 +42,23 @@ int main(int argc, char * argv[]) 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); @@ -68,11 +81,13 @@ int main(int argc, char * argv[]) 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