diff --git a/README.md b/README.md index 33f9085..0c64463 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,7 @@ # compLIB -Rewrite for ROS is the live packaged version since 18.07.2023. - # Dependencies -TODO: document - ## Building documentation ``` pip install sphinx-rtd-theme diff --git a/compLib/CMakeLists.txt b/compLib/CMakeLists.txt index d539099..113ea36 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 src/controls.cpp) +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) diff --git a/compLib/include/controls.h b/compLib/include/controls.h deleted file mode 100644 index 24a3780..0000000 --- a/compLib/include/controls.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef CONTROLS_H -#define CONTROLS_H - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - -#include "irobot_create_msgs/msg/interface_buttons.hpp" -#include "irobot_create_msgs/msg/lightring_leds.hpp" - -class ButtonPressNode : public rclcpp::Node -{ -public: - ButtonPressNode(); - void bt1_wait(); - void bt2_wait(); - void kill(); -private: - void result_callback(const irobot_create_msgs::msg::InterfaceButtons::SharedPtr result); - rclcpp::Subscription::SharedPtr interface_buttons_subscriber_; - bool button1{false}; - bool button2{false}; -}; - -#endif \ No newline at end of file diff --git a/compLib/include/motor.h b/compLib/include/motor.h index a2096c0..24e6ce4 100644 --- a/compLib/include/motor.h +++ b/compLib/include/motor.h @@ -10,7 +10,6 @@ #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 @@ -52,16 +51,4 @@ 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/controls.cpp b/compLib/src/controls.cpp deleted file mode 100644 index a4a84a5..0000000 --- a/compLib/src/controls.cpp +++ /dev/null @@ -1,53 +0,0 @@ -#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 5c6296f..87ba18b 100644 --- a/compLib/src/main.cpp +++ b/compLib/src/main.cpp @@ -4,7 +4,6 @@ #include #include "motor.h" -#include "controls.h" std::mutex action_mutex; @@ -24,16 +23,6 @@ void run_node2(std::shared_ptr node) rclcpp::spin(node); } -void run_node3(std::shared_ptr node) -{ - rclcpp::spin(node); -} - -void run_node4(std::shared_ptr node) -{ - rclcpp::spin(node); -} - int main(int argc, char * argv[]) { rclcpp::init(argc, argv); @@ -41,26 +30,16 @@ 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(); - 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); + ssn->drive(0.2); std::this_thread::sleep_for (std::chrono::milliseconds(2000)); @@ -75,19 +54,13 @@ 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(); - bpn->kill(); t.join(); t1.join(); t2.join(); - t3.join(); - t4.join(); return 0; } \ No newline at end of file diff --git a/compLib/src/motor.cpp b/compLib/src/motor.cpp index a0e4528..8adef43 100644 --- a/compLib/src/motor.cpp +++ b/compLib/src/motor.cpp @@ -10,7 +10,6 @@ #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" @@ -29,7 +28,7 @@ DriveDistNode::DriveDistNode() void DriveDistNode::drive_dist(float meters, float velocity) { - RCLCPP_INFO(this->get_logger(), "drive dist"); + std::cout << "drive dist" << std::endl; processing = true; auto data = irobot_create_msgs::action::DriveDistance::Goal(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); @@ -46,9 +45,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"); @@ -64,7 +63,7 @@ void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandleget_logger(), "DriveDistNode killed"); + std::cout << "DriveDistNode killed" << std::endl; rclcpp::shutdown(); } @@ -79,6 +78,7 @@ 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) { - RCLCPP_INFO(this->get_logger(), "Start drive"); + 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; - RCLCPP_INFO(this->get_logger(), "Stop drive"); } void SetSpeedNode::kill() { - RCLCPP_INFO(this->get_logger(), "SetSpeedNode killed"); + std::cout << "SetSpeedNode killed" << std::endl; 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,60 +166,6 @@ 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"); + std::cout << "RotateAngleNode killed" << std::endl; rclcpp::shutdown(); } \ No newline at end of file