From fa4cc1a0ad7a7ecb6d2a06599c5e7a41c7ca17c0 Mon Sep 17 00:00:00 2001 From: Joel Klimont Date: Fri, 10 Feb 2023 14:29:13 +0100 Subject: [PATCH 1/6] 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 2/6] 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 3/6] (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 4/6] (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 5/6] 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 6/6] 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