Feature/gpio (#132)

TODO explain
trust me bro

# PR Summary
PR Link: INSERT-LINK-HERE

Issue Link: INSERT-LINK-HERE

### Description
Add a single line summary describing the purpose of this PR.

### Reviewers
Tag reviewers.

- Required:
  
- Optional:

---
### Changelog
- Add a bulleted list of major changes

### Reviewer Guide
This is the most important part!
- No one is going to read every line of every PR, so you need to tell
the reviewers what they are looking for.
- Point out lines you want feedback on or feel unsure about.
- Highlight major changes that other members need to know about.

### Testing
#### Automatic
- Describe test cases that are covered by unit tests
#### Manual
- Describe any manual testing (launch files, visualizations, etc.)

### Documentation
- Link any relevant documentation

### Checklist
- [ ] Confirmed all tests pass on a clean build
- [ ] Added reviewers in Github
- [ ] Posted PR Summary to Discord PR's Channel
- [ ] Ran uncrustify on any modified C++ files
- [ ] Ran Colcon Lint for any modified CMakeLists.txt or Package.xml

---------

Co-authored-by: Omega Jerry <[email protected]>

03_ROS/ghost_io/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+project(ghost_io)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+
+
+######################
+#### Dependencies ####
+######################
+# Set Package Dependencies
+set(DEPENDENCIES
+  rclcpp
+  std_msgs
+  )
+
+foreach(pkg ${DEPENDENCIES})
+  find_package(${pkg} REQUIRED)
+endforeach()
+ament_export_dependencies(${DEPENDENCIES})
+
+# Set Include Directories
+set(INCLUDE
+  include
+)
+
+include_directories(${INCLUDE})
+ament_export_include_directories(${INCLUDE})
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+# GPIO Expander
+add_executable(gpio_expander
+  src/PCF8575.cpp
+  src/gpio_expander.cpp
+  #src/tcs_color_sensor.cpp
+)
+ament_target_dependencies(gpio_expander
+  ${DEPENDENCIES}
+)
+target_link_libraries(gpio_expander i2c)
+target_include_directories(gpio_expander
+  PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+install(TARGETS
+  gpio_expander
+  DESTINATION lib/${PROJECT_NAME})
+
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

03_ROS/ghost_io/include/PCF8575.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <cstdint>
+#include <string>
+
+class PCF8575 {
+public:
+    PCF8575(){};
+    /**
+     * @brief Construct a new PCF8575 object.
+     * 
+     * @param i2cBus Path to the I2C bus device (e.g. "/dev/i2c-1").
+     * @param address I2C address of the PCF8575 (typically between 0x20 and 0x27).
+     */
+    bool open(const std::string &i2cBus, uint8_t address, uint16_t initVal = 0xFFFF);
+
+    /// Clean up and close the bus.
+    ~PCF8575();
+
+    /**
+     * @brief Set a single pin to HIGH or LOW.
+     * 
+     * @param pin Pin number (0..15).
+     * @param value true for HIGH, false for LOW.
+     * @return true on success, false on error.
+     */
+    bool setPin(uint8_t pin, bool value);
+
+    /**
+     * @brief Read a single pin value.
+     * 
+     * @param pin Pin number (0..15).
+     * @param value Reference to store the pin value (true for HIGH, false for LOW).
+     * @return true on success, false on error.
+     */
+    bool getPin(uint8_t pin, bool &value);
+
+    bool poll();
+
+private:
+    /**
+     * @brief Read 16 bits from the expander.
+     * 
+     * @return uint16_t 16-bit input value.
+     */
+    uint16_t read16();
+
+    /**
+     * @brief Write 16 bits to the expander.
+     * 
+     * @param value 16-bit output value.
+     * @return true on success, false on error.
+     */
+    bool write16(uint16_t value);
+
+
+    int _fd;           // File descriptor for the I2C bus.
+    uint8_t _address;  // I2C address of the PCF8575.
+
+    uint16_t current_in;
+    uint16_t current_out;
+};

03_ROS/ghost_io/include/ghost_io/gpio_expander.hpp

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "PCF8575.hpp"
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/int64.hpp>
+
+#include <map>
+#include <string>
+#include <vector>
+#include <set>  // Added to support std::set
+
+
+namespace ghost_io
+{
+struct GPIODevice
+{
+  std::vector<int64_t> pins;
+  bool output; // output to world
+  rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr sub;
+  rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr pub;
+};
+
+class GPIOExpander : public rclcpp::Node
+{
+  std::unique_ptr<PCF8575> chip;
+
+  double m_poll_freq;
+
+  std::map<std::string, GPIODevice> m_gpio_map;
+  uint16_t output;
+
+  rclcpp::TimerBase::SharedPtr m_publish_timer;
+
+  void load_gpio_parameters();
+
+public:
+  GPIOExpander();
+
+
+  void poll();
+  void callback(const std_msgs::msg::Int64::SharedPtr in, std::string name);
+
+};
+
+}

03_ROS/ghost_io/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ghost_io</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">karmanyaahm</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

03_ROS/ghost_io/src/PCF8575.cpp

@@ -0,0 +1,84 @@
+#include "PCF8575.hpp"
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+#include <cstdio>
+
+bool PCF8575::open(const std::string & i2cBus, uint8_t address, uint16_t initVal)
+{
+	printf("YO\n");
+  _address = address;
+
+	printf("YO2\n");
+  _fd = ::open(i2cBus.c_str(), O_RDWR);
+  if (_fd < 0) {
+    perror("PCF8575: Failed to open I2C bus");
+	return false;
+  }
+	printf("YO3\n");
+  if (ioctl(_fd, I2C_SLAVE, _address) < 0) {
+    perror("PCF8575: Failed to set I2C address");
+	return false;
+  }
+  current_out = initVal;
+  return true;
+}
+
+
+PCF8575::~PCF8575()
+{
+  if (_fd >= 0) {
+    close(_fd);
+  }
+}
+
+uint16_t PCF8575::read16()
+{
+  uint8_t data[2];
+  int ret = ::read(_fd, data, 2);
+  if (ret != 2) {
+    perror("PCF8575: Failed to read 2 bytes");
+    return 0;
+  }
+  // Combine low and high byte into a 16-bit value.
+  return data[0] | (data[1] << 8);
+}
+
+bool PCF8575::write16(uint16_t value)
+{
+  uint8_t data[2];
+  data[0] = value & 0xFF;            // low byte
+  data[1] = (value >> 8) & 0xFF;       // high byte
+  // TODO: ^^ is so fucking dumb, chatgpt wrote it, todo fix using cast assignment or smth
+  int ret = ::write(_fd, data, 2);
+  if (ret != 2) {
+    perror("PCF8575: Failed to write 2 bytes");
+    return false;
+  }
+  return true;
+}
+
+bool PCF8575::setPin(uint8_t pin, bool value)
+{
+  if (pin > 15) {return false;}
+  if (value) {
+    current_out |= (1 << pin);
+  } else {
+    current_out &= ~(1 << pin);
+  }
+  return true;
+}
+
+bool PCF8575::getPin(uint8_t pin, bool & value)
+{
+  if (pin > 15) {return false;}
+  value = (current_in >> pin) & 0x01;
+  return true;
+}
+
+bool PCF8575::poll()
+{
+  current_in = read16();
+  return write16(current_out);
+}