Skip to content

Commit

Permalink
98 task thruster interface (#111)
Browse files Browse the repository at this point in the history 
* moved thruster interface to ROS2, needs cleanup and proper tests

* Committing clang-format changes

* added config directory to install in CMake

* Committing clang-format changes

* changed subscriber and publisher name

* Committing clang-format changes

* Limited pwm more to 1400-1600

* Fixed thrusters jesjes :)

* Added launch file

* Added node startup log message

* Committing clang-format changes

* removed non-functioning test file.

* small fix

---------

Co-authored-by: Clang Robot <[email protected]>
Co-authored-by: PizzaAllTheWay <[email protected]>
Co-authored-by: RoseKapps <[email protected]>
Co-authored-by: Martin Huynh <[email protected]>
  • Loading branch information
@alekskl01 @PizzaAllTheWay
@RoseKapps @Mokaz
5 people authored Nov 12, 2023
1 parent 29d46be commit 2dfa734
Show file tree
Hide file tree
Showing 8 changed files with 526 additions and 0 deletions.
65 changes: 65 additions & 0 deletions motion/thruster_interface/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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()
190 changes: 190 additions & 0 deletions motion/thruster_interface/config/ThrustMe_P1000_force_mapping.csv
View file
Original file line number Diff line number Diff line change
@@ -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
31 changes: 31 additions & 0 deletions motion/thruster_interface/include/thruster_interface/thruster_interface.hpp
View file
Original file line number Diff line number Diff line change
@@ -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);
};
31 changes: 31 additions & 0 deletions motion/thruster_interface/include/thruster_interface/thruster_interface_node.hpp
View file
Original file line number Diff line number Diff line change
@@ -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);
};
16 changes: 16 additions & 0 deletions motion/thruster_interface/launch/thruster_interface.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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
])
24 changes: 24 additions & 0 deletions motion/thruster_interface/package.xml
View file
Original file line number Diff line number Diff line change
@@ -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>
Loading

0 comments on commit 2dfa734

Please sign in to comment.