98 task thruster interface

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()

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

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);
+};

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);
+};

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
+    ])

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="[email protected]">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>