Waterwitch i2c master

compose.yaml

@@ -13,8 +13,6 @@ services:
     network_mode: host # Run in host mode to allow ros2 communication with host network
     ipc: host # Required for ros2 communication with host network
     privileged: true # Required for access to the i2c bus
-    ports:
-      - "9090:9090"
     volumes:
       - /dev:/dev # Allow access to the i2c bus
       - ./ros_workspace/src/common_backend/common_backend/config:/app/src/common_backend/common_backend/config # Mount the config directory so that it persists between container restarts

ros_workspace/Dockerfile

@@ -11,9 +11,9 @@ ENV DEV_MODE=${DEV_MODE}
 
 RUN if [ "$DEV_MODE" = "false" ]; then \
     # For real ROV (compiles on Raspberry Pi)
-    apt-get update && apt-get install -y python3 python3-pip ros-jazzy-rosbridge-server ros-jazzy-web-video-server ffmpeg libsm6 libxext6 dos2unix libglfw3 libglfw3-dev && \
+    apt-get update && apt-get install -y python3 python3-pip ros-jazzy-rosbridge-server ros-jazzy-web-video-server ffmpeg libsm6 libxext6 dos2unix libglfw3 libglfw3-dev i2c-tools && \
     # TODO: It seems this method of installing Python packages doesn't work on Ubuntu Nobel
-    pip3 install sqlalchemy RPi.GPIO smbus2 adafruit-circuitpython-bno055 adafruit-circuitpython-pca9685 adafruit-circuitpython-servokit opencv-python; \
+    pip3 install --break-system-packages sqlalchemy RPi.GPIO smbus2 adafruit-circuitpython-bno055 adafruit-circuitpython-pca9685 adafruit-circuitpython-servokit opencv-python; \
 else \
     # For development (to interface with Simulator)
     apt-get update && apt-get install -y python3 python3-pip ros-jazzy-rosbridge-server ros-jazzy-web-video-server ffmpeg libsm6 libxext6 dos2unix libglfw3 libglfw3-dev && \
@@ -25,7 +25,6 @@ COPY src /app/src
 COPY docker_entry_script.sh /app
 
 ARG CACHEBUST=1
-
 RUN . /opt/ros/$ROS_DISTRO/setup.sh && colcon build
 RUN echo 'source /app/install/setup.bash $ROV_NAME $DEV_MODE' >> /etc/bash.bashrc
 RUN dos2unix /app/docker_entry_script.sh

ros_workspace/src/waterwitch_backend/CMakeLists.txt

@@ -23,16 +23,20 @@ add_library(waterwitch_constants STATIC ${CMAKE_CURRENT_SOURCE_DIR}/src/waterwit
 
 add_executable(pilot_listener ${CMAKE_CURRENT_SOURCE_DIR}/src/pilot_listener.cpp)
 add_executable(simulation_bot_control ${CMAKE_CURRENT_SOURCE_DIR}/src/simulation_bot_control.cpp)
+add_executable(i2c_master ${CMAKE_CURRENT_SOURCE_DIR}/src/i2c_master.cpp)
 
 ament_target_dependencies(simulation_bot_control rclcpp std_msgs eer_interfaces)
 ament_target_dependencies(pilot_listener rclcpp std_msgs eer_interfaces)
+ament_target_dependencies(i2c_master rclcpp std_msgs eer_interfaces)
 
 target_link_libraries(pilot_listener "${cpp_typesupport_target}" waterwitch_constants)
 target_link_libraries(simulation_bot_control "${cpp_typesupport_target}" waterwitch_constants)
+target_link_libraries(i2c_master "${cpp_typesupport_target}" waterwitch_constants)
 
 install(TARGETS
   pilot_listener
   simulation_bot_control
+  i2c_master
   waterwitch_constants
   DESTINATION lib/${PROJECT_NAME}
 )

ros_workspace/src/waterwitch_backend/include/waterwitch_constants.h

@@ -2,6 +2,9 @@
 #define WATERWITCH_CONSTANTS_H
 
 #include <string>
+#include <unordered_map>
+#include <cstdint>
+
 
 enum CONTROL_AXES {
     SURGE = 0,
@@ -18,9 +21,14 @@ extern const int8_t THRUSTER_CONFIG_MATRIX[6][6];
 // Names of the thrusters
 extern const std::string THRUSTER_NAMES[6];
 
+// Map from thruster name to thruster address on RP2040
+extern const std::unordered_map<std::string, uint8_t> THRUSTER_MAP;
+
 // The maximum amount that the thrusters' thrust value changes in one iteration
 extern const float THRUSTER_ACCELERATION;
 
+extern const int RP2040_ADDRESS;
+
 // The rate at which we send control input TO the board in milliseconds
 extern const int SOFTWARE_TO_BOARD_COMMUNICATION_RATE;
 

ros_workspace/src/waterwitch_backend/launch/waterwitch_startup.xml

@@ -1,6 +1,6 @@
 <launch>
 	<node pkg="waterwitch_backend" exec="pilot_listener"/>
-    <!-- TODO: A node that listens to thruster values and publishes communications on the i2c bus -->
+	<node pkg="waterwitch_backend" exec="i2c_master"/>
 	<node pkg="common_backend" exec="profiles_manager.py"/>
 	<include file="$(find-pkg-share rosbridge_server)/launch/rosbridge_websocket_launch.xml"/>
 </launch>

ros_workspace/src/waterwitch_backend/src/i2c_master.cpp

@@ -0,0 +1,91 @@
+#include <memory>
+#include <thread>
+#include <vector>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <linux/i2c-dev.h>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+#include "std_msgs/msg/float64.hpp"
+#include "eer_interfaces/msg/waterwitch_control.hpp"
+#include "waterwitch_constants.h"
+
+#define I2C_BUS "/dev/i2c-1"
+
+class I2CMaster : public rclcpp::Node
+{
+public:
+  I2CMaster() : Node("I2CMaster")
+  {
+
+    auto control_values_subscriber_callback =
+      [this](eer_interfaces::msg::WaterwitchControl::UniquePtr control_values_msg) -> void {
+        for (int thruster_index = 0; thruster_index < 6; thruster_index++) {
+
+            // Map the thrust value from the range [-1, 1] to [0, 255]
+            uint64_t thrust = static_cast<uint8_t>((control_values_msg->thrust[thruster_index] + 1) * 127.5);
+
+
+            // Publish on i2c bus
+            write_to_i2c(RP2040_ADDRESS, THRUSTER_MAP.at(THRUSTER_NAMES[thruster_index]), thrust, 2);
+        }
+      };
+
+    control_values_subscriber =
+      this->create_subscription<eer_interfaces::msg::WaterwitchControl>(
+        "waterwitch_control_values", 10, control_values_subscriber_callback);
+  }
+
+private:
+  rclcpp::Subscription<eer_interfaces::msg::WaterwitchControl>::SharedPtr control_values_subscriber;
+
+  void write_to_i2c(int device_address, uint8_t byte_1, uint8_t byte_2, uint8_t num_bytes)
+  {
+    int file;
+    if ((file = open(I2C_BUS, O_RDWR)) < 0) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to open the i2c bus");
+      return;
+    }
+
+    if (ioctl(file, I2C_SLAVE, device_address) < 0) {
+      RCLCPP_ERROR(this->get_logger(), "Failed to acquire bus access and/or talk to slave");
+      close(file);
+      return;
+    }
+
+    if (num_bytes == 1)
+    {
+      uint8_t buffer[1];
+      buffer[0] = byte_1;
+      if (write(file, buffer, 1) != 1) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to write to the i2c bus");
+      }
+    }
+    else if (num_bytes == 2)
+    {
+      uint8_t buffer[2];
+      buffer[0] = byte_1;
+      buffer[1] = byte_2;
+      if (write(file, buffer, 2) != 2) {
+        RCLCPP_ERROR(this->get_logger(), "Failed to write to the i2c bus");
+      }
+      else 
+      {
+        RCLCPP_INFO(this->get_logger(), "Wrote to Device Address: 0x%02X, Byte 1: 0x%02X, Byte 2: 0x%02X", device_address, byte_1, byte_2);
+      }
+    }
+
+    close(file);
+  }
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<I2CMaster>());
+  rclcpp::shutdown();
+  return 0;
+}

ros_workspace/src/waterwitch_backend/src/waterwitch_constants.cpp

@@ -18,6 +18,18 @@ const std::string THRUSTER_NAMES[6] = {
     "port_top"
 };
 
+const std::unordered_map<std::string, uint8_t> THRUSTER_MAP = {
+    {"for_star", 0},
+    {"for_port", 1},
+    {"aft_star", 2},
+    {"aft_port", 3},
+    {"star_top", 4},
+    {"port_top", 5}
+};
+
+
 const float THRUSTER_ACCELERATION = 0.1f;
 
+const int RP2040_ADDRESS = 0x08;
+
 const int SOFTWARE_TO_BOARD_COMMUNICATION_RATE = 100;