Add button functionality

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/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;
 
@@ -28,6 +29,11 @@ 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);
@@ -36,16 +42,23 @@ int main(int argc, char * argv[])
     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);
+
+    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;
 }