button

.gitignore

@@ -104,4 +104,4 @@ AMENT_IGNORE
 # End of https://www.toptal.com/developers/gitignore/api/ros2,visualstudiocode,cmake,c++
 
 .vscode
-.cache
+.cache

src/modelec/CMakeLists.txt

@@ -85,6 +85,14 @@ target_include_directories(arm_controller PUBLIC
   $<INSTALL_INTERFACE:include>
 )
 
+add_executable(button_gpio_controller src/button_gpio_controller.cpp)
+ament_target_dependencies(button_gpio_controller rclcpp std_msgs modelec_interface)
+target_link_libraries(button_gpio_controller ${WIRINGPI_LIB})
+target_include_directories(button_gpio_controller PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 # Install targets
 install(TARGETS
   usb_arduino_listener
@@ -95,6 +103,7 @@ install(TARGETS
   tirette_controller
   solenoid_controller
   arm_controller
+  button_gpio_controller
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/modelec/include/modelec/button_gpio_controller.hpp

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <modelec_interface/srv/new_button.hpp>
+#include <modelec_interface/srv/button.hpp>
+#include <modelec_interface/msg/button.hpp>
+#include <wiringPi.h>
+
+namespace Modelec {
+    class ButtonGpioController : public rclcpp::Node {
+
+        struct Button {
+            int pin;
+            rclcpp::Publisher<modelec_interface::msg::Button>::SharedPtr publisher;
+            std::string name;
+        };
+
+    public:
+        ButtonGpioController();
+
+    private:
+        // service
+        rclcpp::Service<modelec_interface::srv::NewButton>::SharedPtr new_button_service_;
+        rclcpp::Service<modelec_interface::srv::Button>::SharedPtr button_server_;
+
+        // service callbacks
+        void new_button(const std::shared_ptr<modelec_interface::srv::NewButton::Request> request, std::shared_ptr<modelec_interface::srv::NewButton::Response> response);
+        void check_button(const std::shared_ptr<modelec_interface::srv::Button::Request> request, std::shared_ptr<modelec_interface::srv::Button::Response> response);
+
+        // timer
+        rclcpp::TimerBase::SharedPtr timer_;
+
+        std::map<int, Button> buttons_;
+    };
+}

src/modelec/src/button_gpio_controller.cpp

@@ -0,0 +1,57 @@
+#include <modelec/button_gpio_controller.hpp>
+
+namespace Modelec {
+  ButtonGpioController::ButtonGpioController() : Node("button_gpio_controller") {
+    if (wiringPiSetupGpio() == -1)
+    {
+        RCLCPP_ERROR(this->get_logger(), "Failed to initialize GPIO.");
+        rclcpp::shutdown();
+    }
+
+    new_button_service_ = create_service<modelec_interface::srv::NewButton>("new_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2));
+    button_server_ = create_service<modelec_interface::srv::Button>("button", std::bind(&ButtonGpioController::check_button, this, std::placeholders::_1, std::placeholders::_2));
+    timer_ = create_wall_timer(std::chrono::seconds(1), [this]() {
+      for (auto& button : buttons_) {
+        modelec_interface::msg::Button msg;
+        msg.pin = button.second.pin;
+        msg.state = digitalRead(button.second.pin) == LOW;
+        button.second.publisher->publish(msg);
+      }
+    });
+  }
+
+  void ButtonGpioController::new_button(const std::shared_ptr<modelec_interface::srv::NewButton::Request> request, std::shared_ptr<modelec_interface::srv::NewButton::Response> response) {
+    if (buttons_.find(request->pin) != buttons_.end()) {
+      response->success = false;
+      return;
+    }
+
+    Button button;
+    button.pin = request->pin;
+    button.name = request->name;
+    button.publisher = create_publisher<modelec_interface::msg::Button>("button/" + request->name, 10);
+
+    buttons_.insert({request->pin, button});
+
+    pinMode(request->pin, INPUT);
+    pullUpDnControl(request->pin, PUD_UP);
+
+    response->success = true;
+  }
+
+  void ButtonGpioController::check_button(const std::shared_ptr<modelec_interface::srv::Button::Request> request, std::shared_ptr<modelec_interface::srv::Button::Response> response) {
+    if (buttons_.find(request->pin) == buttons_.end()) {
+      response->status = false;
+      return;
+    }
+
+    response->status = digitalRead(request->pin) == LOW;
+  }
+}
+
+int main(int argc, char** argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<Modelec::ButtonGpioController>());
+  rclcpp::shutdown();
+  return 0;
+}

src/modelec_interface/CMakeLists.txt

@@ -16,9 +16,12 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ServoMoteur.msg"
   "msg/ServoMode.msg"
   "msg/Solenoid.msg"
+  "msg/Button.msg"
   "srv/NewServoMotor.srv"
   "srv/NewSolenoid.srv"
   "srv/Tirette.srv"
+  "srv/NewButton.srv"
+  "srv/Button.srv"
   DEPENDENCIES std_msgs
 )
 

src/modelec_interface/msg/Button.msg

@@ -0,0 +1,2 @@
+int64 pin
+bool state

src/modelec_interface/srv/Button.srv

@@ -0,0 +1,4 @@
+int64 pin
+string name
+---
+bool status

src/modelec_interface/srv/NewButton.srv

@@ -0,0 +1,4 @@
+int64 pin
+string name
+---
+bool success