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