From ed04957d94e0067e2e5e58d2b4ec3c9752ce7421 Mon Sep 17 00:00:00 2001 From: Matthias Guzmits Date: Wed, 19 Jul 2023 18:36:07 +0200 Subject: [PATCH 1/2] 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 2/2] 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