From a22233573c63cef9f77a15564c10653c0b74532a Mon Sep 17 00:00:00 2001
From: Vincent Belpois <vincent.belpois@gmail.com>
Date: Wed, 8 May 2024 22:45:36 +0200
Subject: [PATCH] Added first tirette node test

---
 src/ezbot_command_module/CMakeLists.txt       | 16 +++++
 src/ezbot_command_module/include/tirette.hpp  | 50 ++++++++++++++++
 src/ezbot_command_module/src/tirette_node.cpp | 58 +++++++++++++++++++
 3 files changed, 124 insertions(+)
 create mode 100644 src/ezbot_command_module/include/tirette.hpp
 create mode 100644 src/ezbot_command_module/src/tirette_node.cpp

diff --git a/src/ezbot_command_module/CMakeLists.txt b/src/ezbot_command_module/CMakeLists.txt
index 5808de5..5d1ff0f 100644
--- a/src/ezbot_command_module/CMakeLists.txt
+++ b/src/ezbot_command_module/CMakeLists.txt
@@ -17,6 +17,8 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -32,6 +34,20 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+include_directories(include)
+
+
+add_executable(tirette_node src/tirette_node.cpp)
+ament_target_dependencies(tirette_node rclcpp std_msgs)
+
+target_link_libraries(tirette_node gpiod)
+
+install(TARGETS
+  tirette_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
 install(
   DIRECTORY config launch
   DESTINATION share/${PROJECT_NAME}
diff --git a/src/ezbot_command_module/include/tirette.hpp b/src/ezbot_command_module/include/tirette.hpp
new file mode 100644
index 0000000..d6a507c
--- /dev/null
+++ b/src/ezbot_command_module/include/tirette.hpp
@@ -0,0 +1,50 @@
+#ifndef TIRETTE_HPP
+#define TIRETTE_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <gpiod.h>
+
+
+class TiretteNode : public rclcpp::Node
+{
+public:
+    TiretteNode();
+
+private:
+
+    int gpioTirette = 11;
+    rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tirette_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+    bool init_gpiod(void);
+    void readGPIO();
+    
+    struct gpiod_chip  *gpiochip;
+    struct gpiod_line  *gpioline;
+};
+
+
+
+
+bool TiretteNode::init_gpiod(void)
+{
+  gpiochip = gpiod_chip_open_by_name("gpiochip4");
+  if(gpiochip == NULL)
+      gpiochip = gpiod_chip_open_by_name("gpiochip0");
+  if(gpiochip == NULL)
+      {
+           printf("unable to open GPIO\n");
+           return false;
+      }
+
+    gpioline = gpiod_chip_get_line(gpiochip,gpioTirette);
+
+    // set  gpio pin INPUT PULL UP 
+    gpiod_line_request_input_flags(gpioline,"MyBlink", GPIOD_LINE_BIAS_PULL_UP);
+
+    return true;
+
+}
+
+
+#endif // TIRETTE_HPP
\ No newline at end of file
diff --git a/src/ezbot_command_module/src/tirette_node.cpp b/src/ezbot_command_module/src/tirette_node.cpp
new file mode 100644
index 0000000..b654317
--- /dev/null
+++ b/src/ezbot_command_module/src/tirette_node.cpp
@@ -0,0 +1,58 @@
+#include "tirette.hpp"
+
+
+
+void TiretteNode::readGPIO()
+{
+    auto message = std_msgs::msg::Bool();
+
+    struct gpiod_chip *chip;
+    struct gpiod_line *line;
+    int value;
+
+    chip = gpiod_chip_open("/dev/gpiochip0");
+    if (!chip) {
+        // handle error
+    }
+
+    line = gpiod_chip_get_line(chip, 11);
+    if (!line) {
+        // handle error
+    }
+
+    if (gpiod_line_request_input(line, "tirette_node") < 0) {
+        // handle error
+    }
+
+    value = gpiod_line_get_value(line);
+    if (value < 0) {
+        // handle error
+    }
+
+    message.data = value ? true : false;
+
+    tirette_pub_->publish(message);
+
+    gpiod_line_release(line);
+    gpiod_chip_close(chip);
+}
+TiretteNode::TiretteNode() : Node("tirette_node")
+{
+
+   init_gpiod();
+    tirette_pub_ = this->create_publisher<std_msgs::msg::Bool>("tirette_status", 10);
+    timer_ = this->create_wall_timer(
+        std::chrono::seconds(1),
+        std::bind(&TiretteNode::readGPIO, this));
+
+
+}
+
+
+int main(int argc, char *argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<TiretteNode>());
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file