Merge branch 'master' of https://github.com/F-WuTS/compLIB

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)

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<irobot_create_msgs::msg::InterfaceButtons>::SharedPtr interface_buttons_subscriber_;
+    bool button1{false};
+    bool button2{false};
+};
+
+#endif

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<irobot_create_msgs::action::DriveArc>::WrappedResult & result);
+    rclcpp_action::Client<irobot_create_msgs::action::DriveArc>::SharedPtr drive_arc_action_;
+    bool processing;
+};
+
 #endif

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<irobot_create_msgs::msg::InterfaceButtons>(
+        "/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();
+}

compLib/src/main.cpp

@@ -4,6 +4,7 @@
 #include <mutex>
 
 #include "motor.h"
+#include "controls.h"
 
 std::mutex action_mutex;
 
@@ -23,6 +24,16 @@ void run_node2(std::shared_ptr<RotateAngleNode> node)
     rclcpp::spin(node);
 }
 
+void run_node3(std::shared_ptr<DriveArcNode> node)
+{
+    rclcpp::spin(node);
+}
+
+void run_node4(std::shared_ptr<ButtonPressNode> node)
+{
+    rclcpp::spin(node);
+}
+
 int main(int argc, char * argv[])
 {
     rclcpp::init(argc, argv);
@@ -30,16 +41,26 @@ int main(int argc, char * argv[])
     auto ddn = std::make_shared<DriveDistNode>();
     auto ssn = std::make_shared<SetSpeedNode>();
     auto ran = std::make_shared<RotateAngleNode>();
+    auto dan = std::make_shared<DriveArcNode>();
+    auto bpn = std::make_shared<ButtonPressNode>();
     
     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);
 
-    ssn->drive(0.2);
+    bpn->bt1_wait();
+    bpn->bt2_wait();
+    bpn->bt1_wait();
+
+    ssn->drive(0.3);
 
     std::this_thread::sleep_for (std::chrono::milliseconds(2000));
 
@@ -54,13 +75,19 @@ 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;
 }

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<irobot_create_msgs::action::DriveDistance>::SendGoalOptions();
@@ -45,9 +46,9 @@ void DriveDistNode::drive_dist(float meters, float velocity)
 void DriveDistNode::result_callback(const rclcpp_action::ClientGoalHandle<irobot_create_msgs::action::DriveDistance>::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::ClientGoalHandle<irobot
 
 void DriveDistNode::kill()
 {
-    std::cout << "DriveDistNode killed" << std::endl;
+    RCLCPP_INFO(this->get_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<irobot_create_msgs::action::RotateAngle>::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::ClientGoalHandle<irob
 
 void RotateAngleNode::kill()
 {
-    std::cout << "RotateAngleNode killed" << std::endl;
+    RCLCPP_INFO(this->get_logger(), "RotateAngleNode killed");
+    rclcpp::shutdown();
+}
+
+DriveArcNode::DriveArcNode()
+: Node("drive_arc_node")
+{
+    drive_arc_action_ = rclcpp_action::create_client<irobot_create_msgs::action::DriveArc>(
+        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<irobot_create_msgs::action::DriveArc>::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<irobot_create_msgs::action::DriveArc>::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();
 }