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( 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 +#include +#include + + +class TiretteNode : public rclcpp::Node +{ +public: + TiretteNode(); + +private: + + int gpioTirette = 11; + rclcpp::Publisher::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("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()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file