diff --git a/motion/thruster_interface/CMakeLists.txt b/motion/thruster_interface/CMakeLists.txt
new file mode 100644
index 00000000..c89c2538
--- /dev/null
+++ b/motion/thruster_interface/CMakeLists.txt
@@ -0,0 +1,65 @@
+cmake_minimum_required(VERSION 3.8)
+project(thruster_interface)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(vortex_msgs REQUIRED)
+
+include_directories(include)
+
+
+add_executable(${PROJECT_NAME}_node
+  src/thruster_interface.cpp
+  src/thruster_interface_node.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}_node
+  rclcpp
+  std_msgs
+  vortex_msgs
+)
+
+install(
+  DIRECTORY include/
+  DESTINATION include 
+)
+
+# Install launch files.
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME}
+)
+
+install(TARGETS
+  ${PROJECT_NAME}_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+if(BUILD_TESTING)
+
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_add_gtest(${PROJECT_NAME}_test 
+    src/thruster_interface.cpp
+  )
+  target_include_directories(${PROJECT_NAME}_test PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  ament_target_dependencies(${PROJECT_NAME}_test
+    std_msgs
+  )
+endif()
+
+ament_package()
diff --git a/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv
new file mode 100644
index 00000000..bac8cf21
--- /dev/null
+++ b/motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv
@@ -0,0 +1,190 @@
+"Force(g)","PWM(micros)"
+-12989.13	1100
+-12792.53	1104.49
+-12779.61	1108.99
+-12635.04	1113.48
+-12547.48	1117.98
+-12554.38	1122.47
+-12528.53	1126.97
+-12484.77	1131.46
+-12327.03	1135.96
+-12337.55	1140.45
+-12202.25	1144.94
+-12147.19	1149.44
+-12097.76	1153.93
+-11947.61	1158.43
+-11948.59	1162.92
+-11878.31	1167.42
+-11685.45	1171.91
+-11630.87	1176.4
+-11660.74	1180.9
+-11514.6	1185.39
+-11370.6	1189.89
+-11293.55	1194.38
+-11221.2	1198.88
+-11213.27	1203.37
+-11064.32	1207.87
+-11049.23	1212.36
+-10945.55	1216.85
+-10837.99	1221.35
+-10678.96	1225.84
+-10754.31	1230.34
+-10588.09	1234.83
+-10503.28	1239.33
+-10344.9	1243.82
+-10226.64	1248.31
+-10190.24	1252.81
+-10081.85	1257.3
+-9959.01	1261.8
+-9948.11	1266.29
+-9759.36	1270.79
+-9753.3	1275.28
+-9619.75	1279.78
+-9525.81	1284.27
+-9362.72	1288.76
+-9333.51	1293.26
+-9187.35	1297.75
+-9078.53	1302.25
+-8851.82	1306.74
+-8732.14	1311.24
+-8632.45	1315.73
+-8490.62	1320.22
+-8411.14	1324.72
+-8312.52	1329.21
+-8047.98	1333.71
+-7948.19	1338.2
+-7934.35	1342.7
+-7780.13	1347.19
+-7709.89	1351.69
+-7375.4	1356.18
+-7364.05	1360.67
+-7233.1	1365.17
+-6972.58	1369.66
+-6845.09	1374.16
+-6759.8	1378.65
+-6633.6	1383.15
+-6337.56	1387.64
+-6224.88	1392.13
+-6132.13	1396.63
+-5939.88	1401.12
+-5682.91	1405.62
+-5527.56	1410.11
+-5258.96	1414.61
+-5074.12	1419.1
+-4823.63	1423.6
+-4685.91	1428.09
+-4500.56	1432.58
+-4308.91	1437.08
+-4012.16	1441.57
+-3866.78	1446.07
+-3608.86	1450.56
+-3235.68	1455.06
+-2964.98	1459.55
+-2782.91	1464.04
+-2522.36	1468.54
+-2112.45	1473.03
+-1955.09	1477.53
+-1551.28	1482.02
+-1327.8	1486.52
+-913.3	1491.01
+-506.29	1495.51
+0	1500
+614.46	1504.04
+967.25	1508.08
+1501.92	1512.12
+1965.98	1516.16
+2315.18	1520.2
+2789.34	1524.24
+3218.91	1528.28
+3442.74	1532.32
+3763.95	1536.36
+4118.76	1540.4
+4473.32	1544.44
+4733.06	1548.48
+5052.11	1552.53
+5306.67	1556.57
+5514.31	1560.61
+5789.63	1564.65
+6061.47	1568.69
+6337.72	1572.73
+6516.65	1576.77
+6718.8	1580.81
+6901.18	1584.85
+7112.16	1588.89
+7352	1592.93
+7634.54	1596.97
+7720.88	1601.01
+7973.84	1605.05
+8136.83	1609.09
+8386.24	1613.13
+8569.13	1617.17
+8745.66	1621.21
+8921.09	1625.25
+9081.5	1629.29
+9306.64	1633.33
+9397.61	1637.37
+9521.35	1641.41
+9746.14	1645.45
+9890.84	1649.49
+9952.96	1653.54
+10218.41	1657.58
+10275.6	1661.62
+10446.95	1665.66
+10584.48	1669.7
+10754.31	1673.74
+10864.61	1677.78
+10959.78	1681.82
+11109.23	1685.86
+11210.52	1689.9
+11367.21	1693.94
+11528.98	1697.98
+11604.96	1702.02
+11700.44	1706.06
+11756.9	1710.1
+11942.16	1714.14
+12045.06	1718.18
+12179.5	1722.22
+12383.12	1726.26
+12317.95	1730.3
+12531.05	1734.34
+12581.61	1738.38
+12716.21	1742.42
+12825.03	1746.46
+12832.83	1750.51
+13053.31	1754.55
+13181.88	1758.59
+13207.91	1762.63
+13247.19	1766.67
+13414.53	1770.71
+13466.28	1774.75
+13573.65	1778.79
+13696.17	1782.83
+13803.27	1786.87
+13847.3	1790.91
+14029.66	1794.95
+14067.08	1798.99
+14156.74	1803.03
+14283.08	1807.07
+14300.44	1811.11
+14443.34	1815.15
+14541.8	1819.19
+14560.88	1823.23
+14584.52	1827.27
+14665.63	1831.31
+14718.97	1835.35
+14901.01	1839.39
+14973.02	1843.43
+15054.56	1847.47
+15149.7	1851.52
+15239.98	1855.56
+15272.75	1859.6
+15281.37	1863.64
+15384.03	1867.68
+15419.57	1871.72
+15593.75	1875.76
+15602.55	1879.8
+15724.38	1883.84
+15696.74	1887.88
+15764.41	1891.92
+15910.14	1895.96
+15958.35	1900
\ No newline at end of file
diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp
new file mode 100644
index 00000000..8367eb68
--- /dev/null
+++ b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <chrono>
+#include <cmath>
+#include <cstdint>
+#include <fcntl.h>
+#include <fstream>
+#include <iostream>
+#include <linux/i2c-dev.h>
+#include <map>
+#include <sstream>
+#include <string>
+#include <sys/ioctl.h>
+#include <thread>
+#include <unistd.h>
+#include <vector>
+
+class ThrusterInterface {
+private:
+  std::map<double, double> pwm_table;
+  const int I2C_BUS = 1;
+  const int I2C_ADDRESS = 0x21;
+  const char *I2C_DEVICE = "/dev/i2c-1";
+
+  std::vector<uint8_t> pwm_to_bytes(const std::vector<int> &pwm_values);
+
+public:
+  ThrusterInterface(std::string mapping_file);
+  void publish_thrust_to_escs(std::vector<double> forces);
+  float interpolate(float force);
+};
diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp
new file mode 100644
index 00000000..6e4f0d65
--- /dev/null
+++ b/motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <vortex_msgs/msg/pwm.hpp>
+#include <vortex_msgs/msg/thruster_forces.hpp>
+
+#include <thruster_interface/thruster_interface.hpp>
+using std::placeholders::_1;
+
+class ThrusterInterfaceROS : public rclcpp::Node {
+private:
+  std::string mapping_file =
+      ament_index_cpp::get_package_share_directory("thruster_interface") +
+      "/config/ThrustMe_P1000_force_mapping.csv";
+  ThrusterInterface thrusterInterface{mapping_file};
+
+  rclcpp::Subscription<vortex_msgs::msg::ThrusterForces>::SharedPtr
+      thruster_forces_sub_;
+  rclcpp::Publisher<vortex_msgs::msg::Pwm>::SharedPtr pwm_pub_;
+
+public:
+  ThrusterInterfaceROS() : Node("thruster_interface") {
+    pwm_pub_ = this->create_publisher<vortex_msgs::msg::Pwm>("pwm", 10);
+    thruster_forces_sub_ =
+        this->create_subscription<vortex_msgs::msg::ThrusterForces>(
+            "thrust/thruster_forces", 10,
+            std::bind(&ThrusterInterfaceROS::thrust_callback, this, _1));
+  }
+  void thrust_callback(const vortex_msgs::msg::ThrusterForces::SharedPtr msg);
+};
diff --git a/motion/thruster_interface/launch/thruster_interface.launch.py b/motion/thruster_interface/launch/thruster_interface.launch.py
new file mode 100644
index 00000000..c8e88718
--- /dev/null
+++ b/motion/thruster_interface/launch/thruster_interface.launch.py
@@ -0,0 +1,16 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    thruster_interface_node = Node(
+            package='thruster_interface',
+            executable='thruster_interface_node',
+            name='thruster_interface_node',
+            parameters=[],
+            output='screen',
+        )
+    return LaunchDescription([
+        thruster_interface_node
+    ])
\ No newline at end of file
diff --git a/motion/thruster_interface/package.xml b/motion/thruster_interface/package.xml
new file mode 100644
index 00000000..af20df9f
--- /dev/null
+++ b/motion/thruster_interface/package.xml
@@ -0,0 +1,24 @@
+<?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>thruster_interface</name>
+  <version>0.0.0</version>
+  <description>Interface for transforming thrust to thruster rpm</description>
+  <maintainer email="aleksander.klu@hotmail.com">alekskl01</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>vortex_msgs</depend>
+  <depend>ament_index_cpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/motion/thruster_interface/src/thruster_interface.cpp b/motion/thruster_interface/src/thruster_interface.cpp
new file mode 100644
index 00000000..3e1741d0
--- /dev/null
+++ b/motion/thruster_interface/src/thruster_interface.cpp
@@ -0,0 +1,114 @@
+#include <thruster_interface/thruster_interface.hpp>
+
+ThrusterInterface::ThrusterInterface(std::string mapping_file) {
+  // Open the data file
+  std::ifstream data(mapping_file);
+
+  if (!data.is_open()) {
+    std::cerr << "Unable to open file\n";
+    exit(1);
+  }
+
+  std::string line;
+  // Ignore the header line
+  std::getline(data, line);
+
+  while (std::getline(data, line)) {
+    std::istringstream stream(line);
+
+    std::string force_str, pwm_str;
+
+    std::getline(stream, force_str, '\t');
+    std::getline(stream, pwm_str);
+
+    double force = std::stod(force_str);
+    double pwm = std::stod(pwm_str);
+
+    pwm_table[force] = pwm;
+  }
+}
+
+float ThrusterInterface::interpolate(float force) {
+  auto upper_bound = pwm_table.upper_bound(force);
+  if (upper_bound == pwm_table.end()) {
+    --upper_bound;
+  }
+  auto lower_bound = std::prev(upper_bound);
+
+  if (lower_bound == pwm_table.end()) {
+    return upper_bound->second;
+  }
+
+  float force1 = lower_bound->first;
+  float force2 = upper_bound->first;
+
+  float pwm1 = lower_bound->second;
+  float pwm2 = upper_bound->second;
+
+  if (force1 == force2) {
+    return pwm1;
+  }
+
+  int pwm_signal = std::round(
+      pwm1 + ((force - force1) * (pwm2 - pwm1)) / (force2 - force1) + 0.5);
+
+  int clipped_pwm_signal =
+      std::min(std::max(pwm_signal, 1400), 1600); // min 1100, max 1900
+
+  return clipped_pwm_signal;
+}
+
+std::vector<uint8_t>
+ThrusterInterface::pwm_to_bytes(const std::vector<int> &pwm_values) {
+
+  std::vector<uint8_t> bytes;
+  for (const auto &val : pwm_values) {
+    // Ensure the value is in the correct range and cast to an integer
+    int pwm_int = static_cast<int>(
+        std::min(std::max(val, 1400), 1600)); // min 1100, max 1900
+
+    // Split the integer into most significant byte (MSB) and least significant
+    // byte (LSB)
+    uint8_t msb = (pwm_int >> 8) & 0xFF;
+    uint8_t lsb = pwm_int & 0xFF;
+
+    // Add MSB and LSB to the bytes vector
+    bytes.push_back(msb);
+    bytes.push_back(lsb);
+  }
+  return bytes;
+}
+
+void ThrusterInterface::publish_thrust_to_escs(std::vector<double> forces) {
+  std::vector<int> pwm_values;
+  for (auto force : forces) {
+    pwm_values.push_back(interpolate(force));
+  }
+
+  std::vector<uint8_t> pwm_bytes = pwm_to_bytes(pwm_values);
+  int data_size = pwm_bytes.size();
+
+  // DEBUG
+  // for (auto pwm : pwm_values) {
+  //    std::cout << pwm << " ";
+  //}
+  // std::cout << std::endl;
+
+  int file = open(I2C_DEVICE, O_RDWR);
+  if (file < 0) {
+    std::cerr << "Error opening device\n";
+    exit(1);
+  }
+
+  if (ioctl(file, I2C_SLAVE, I2C_ADDRESS) < 0) {
+    std::cerr << "Error setting I2C address\n";
+    exit(1);
+  }
+
+  // Send the I2C message
+  if (write(file, pwm_bytes.data(), data_size) != data_size) {
+    std::cerr << "Error sending data, ignoring message...\n";
+  }
+
+  close(file);
+}
diff --git a/motion/thruster_interface/src/thruster_interface_node.cpp b/motion/thruster_interface/src/thruster_interface_node.cpp
new file mode 100644
index 00000000..ffcceea2
--- /dev/null
+++ b/motion/thruster_interface/src/thruster_interface_node.cpp
@@ -0,0 +1,55 @@
+#include <thruster_interface/thruster_interface_node.hpp>
+
+void ThrusterInterfaceROS::thrust_callback(
+    const vortex_msgs::msg::ThrusterForces::SharedPtr msg) {
+  // Convert from Newton to grams
+  double newton_to_gram_conversion_factor = 101.97162;
+
+  std::vector<double> forces_in_grams = {
+      msg->thrust[0] * newton_to_gram_conversion_factor,
+      msg->thrust[1] * newton_to_gram_conversion_factor,
+      msg->thrust[2] * newton_to_gram_conversion_factor,
+      msg->thrust[3] * newton_to_gram_conversion_factor};
+
+  std::vector<int> pwm_values;
+  for (auto force : forces_in_grams) {
+    pwm_values.push_back(thrusterInterface.interpolate(force));
+  }
+
+  vortex_msgs::msg::Pwm pwm_msg;
+  // TODO: Get mapping and offsets from rosparam
+  // Give thrust to thruster 0: publish on pin = thruster_to_pin_map[0]
+  std::vector<int> thruster_to_pin_map = {1, 3, 2, 0};
+  std::vector<int> thruster_direction_map = {1, 1, 1, -1};
+  std::vector<int> pwm_offsets = {100, 100, 100, 100};
+
+  // Iterates through thruster 0 to 3, where 0 is front right, iterated
+  // clockwise
+  for (int i = 0; i < 4; i++) {
+
+    int center_pwm_value = 1500;
+    int offset_from_center_value =
+        pwm_values[i] - center_pwm_value + pwm_offsets[i];
+    int pwm_value_correct_direction =
+        center_pwm_value + thruster_direction_map[i] * offset_from_center_value;
+
+    int pwm_clamped = std::min(std::max(pwm_value_correct_direction, 1400),
+                               1600); // min 1100, max 1900
+    pwm_msg.positive_width_us.push_back(pwm_clamped);
+    pwm_msg.pins.push_back(thruster_to_pin_map[i]);
+  }
+
+  pwm_pub_->publish(pwm_msg);
+
+  thrusterInterface.publish_thrust_to_escs(forces_in_grams);
+}
+
+int main(int argc, char *argv[]) {
+  rclcpp::init(argc, argv);
+  auto thruster_interface_node = std::make_shared<ThrusterInterfaceROS>();
+  RCLCPP_INFO(thruster_interface_node->get_logger(),
+              "Starting thruster_interface_node");
+  rclcpp::spin(thruster_interface_node);
+  rclcpp::shutdown();
+  return 0;
+}
\ No newline at end of file