From d2e8d07e961964be5b7c136d5b0e1823da1310db Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Mon, 18 Mar 2024 02:14:46 +0100 Subject: [PATCH] Finalized ROS2 Node, only testing left :) --- asv_setup/CMakeLists.txt | 0 asv_setup/README.md | 0 .../environments/trondheim_freshwater.yaml | 0 .../environments/trondheim_saltwater.yaml | 0 asv_setup/config/robots/freya.yaml | 9 +- asv_setup/package.xml | 0 .../thruster_interface_asv_driver_lib.hpp | 19 +- .../thruster_interface_asv_node.hpp | 8 +- .../launch/thruster_interface_asv.launch.py | 24 ++ motion/thruster_interface_asv/package.xml | 2 + .../src/thruster_interface_asv_driver_lib.cpp | 384 +++++++++--------- .../src/thruster_interface_asv_node.cpp | 197 +++++---- 12 files changed, 369 insertions(+), 274 deletions(-) mode change 100755 => 100644 asv_setup/CMakeLists.txt mode change 100755 => 100644 asv_setup/README.md mode change 100755 => 100644 asv_setup/config/environments/trondheim_freshwater.yaml mode change 100755 => 100644 asv_setup/config/environments/trondheim_saltwater.yaml mode change 100755 => 100644 asv_setup/package.xml create mode 100644 motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py diff --git a/asv_setup/CMakeLists.txt b/asv_setup/CMakeLists.txt old mode 100755 new mode 100644 diff --git a/asv_setup/README.md b/asv_setup/README.md old mode 100755 new mode 100644 diff --git a/asv_setup/config/environments/trondheim_freshwater.yaml b/asv_setup/config/environments/trondheim_freshwater.yaml old mode 100755 new mode 100644 diff --git a/asv_setup/config/environments/trondheim_saltwater.yaml b/asv_setup/config/environments/trondheim_saltwater.yaml old mode 100755 new mode 100644 diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index a3d76e7e..b46900a4 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -41,9 +41,12 @@ rate_of_change: max: 0 # Maximum rate of change in newton per second for a thruster - thruster_to_pin_map: [3, 2, 1, 0] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc.. - direction: [1,-1,-1, 1] # Disclose during thruster mapping - offset: [0, 0, 0, 0] # Disclose during thruster mapping + + thruster_to_pin_mapping: [0, 3, 2, 1] # I.e. if thruster_to_pin = [1, 3, 2, 0] then thruster 0 is pin 1 etc.. + thruster_direction: [1, -1, 1, 1] # Disclose during thruster mapping (+/- 1) + thruster_PWM_offset: [0, 0, 0, 0] # Disclose during thruster mapping, Recomended: [0, 0, 0, 0] + thruster_PWM_min: [1100, 1100, 1100, 1100] # Minimum PWM value, Recomended: [1100, 1100, 1100, 1100] + thruster_PWM_max: [1900, 1900, 1900, 1900] # Maximum PWM value, Recomended: [1900, 1900, 1900, 1900] command: wrench: max: [150.0, 150.0, 150.0, 150.0, 150.0, 150.0] diff --git a/asv_setup/package.xml b/asv_setup/package.xml old mode 100755 new mode 100644 diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp index da784534..71093e1c 100644 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp +++ b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp @@ -1,19 +1,20 @@ // C++ Libraries -#include -#include -#include -#include #include #include #include +#include +#include +#include +#include -#include // Used for the O_RDWR define +#include // Used for the O_RDWR define #include // Used for the I2C_SLAVE define #include -#include // Used for the close function +#include // Used for the close function + // Declare functions we can use namespace thruster_interface_asv_driver_lib { -void init(const std::string &pathToCSVFile, int16_t *minPWM, int16_t *maxPWM); -int16_t *drive_thrusters(float *forces); -} // namespace thruster_interface_asv_driver_lib \ No newline at end of file + void init(const std::string& pathToCSVFile, int8_t* thrusterMapping, int8_t* thrusterDirection, int16_t* offsetPWM, int16_t* minPWM, int16_t* maxPWM); + int16_t* drive_thrusters(float* thrusterForces); +} \ No newline at end of file diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp index 7688ace7..43890911 100644 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp +++ b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp @@ -1,9 +1,13 @@ +// C++ Libraries +#include + // ROS2 libraries -#include -#include // For the ROS2 Timer clock #include #include #include +#include +#include // For the ROS2 Timer clock // Custom libraries #include + diff --git a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py new file mode 100644 index 00000000..ba502edf --- /dev/null +++ b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + # Path to the YAML file + yaml_file_path = os.path.join( + get_package_share_directory("thruster_interface_asv"), + "../../../../", # Go to the workspace + "src/vortex-asv/asv_setup/config/robots/", # Go inside where yamal files are located at + "freya.yaml" + ) + + return LaunchDescription([ + Node( + package='thruster_interface_asv', + namespace='thruster_interface_asv', + executable='thruster_interface_asv_node', + name='thruster_interface_asv_node', + output='screen', + parameters=[yaml_file_path], + ), + ]) \ No newline at end of file diff --git a/motion/thruster_interface_asv/package.xml b/motion/thruster_interface_asv/package.xml index 9f05eb92..8672c824 100644 --- a/motion/thruster_interface_asv/package.xml +++ b/motion/thruster_interface_asv/package.xml @@ -17,6 +17,8 @@ ament_lint_common ament_cmake_gtest + ros2launch + ament_cmake diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp index 131e5c82..9ddd38e9 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp @@ -1,199 +1,219 @@ #include + namespace thruster_interface_asv_driver_lib { -// Interpolation from forces to pwm functions (START) ---------- -struct _ForcePWM { - float force; - float pwm; -}; - -// Local variables used inside the interpolation -std::vector<_ForcePWM> _datatableForForceAndPWM; - -std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { - // Prepare the datastructure we will load our data in - std::vector<_ForcePWM> data; - - // Try opening the file and if suceeded fill our datastructure with all the - // values in the .CSV file - std::ifstream file(filepath); - if (file.is_open()) { - // Variable for the currect line value in the .CSV file - std::string line; - - // Skip the header line - std::getline(file, line); - - // Read data line by line - while (std::getline(file, line)) { - // Temporarty variables to store data correctly ---------- - // Define temporary placeholders for variables we are extracting - std::string tempVar; - // Define data structure format we want our .CSV vlaues to be ordered in - _ForcePWM formatOfForcePWMDatastructure; - - // Data manipulation ---------- - // csvLineSplit variable converts "line" variable to a char stream of data - // that can further be used to split and filter out values we want - std::istringstream csvLineSplit(line); - // Extract Forces from "csvLineSplit" variable - std::getline(csvLineSplit, tempVar, '\t'); - formatOfForcePWMDatastructure.force = std::stof(tempVar); - // Convert grams into Newtons as we expect to get Forces in Newtons but - // the .CSV file calculates forsces in grams - formatOfForcePWMDatastructure.force = - formatOfForcePWMDatastructure.force * (9.81 / 1000.0); - // Extract PWM from "csvLineSplit" variable - std::getline(csvLineSplit, tempVar, '\t'); - formatOfForcePWMDatastructure.pwm = std::stof(tempVar); - - // Push processed data with correct formating into the complete dataset - // ---------- - data.push_back(formatOfForcePWMDatastructure); + // Interpolation from forces to pwm functions (START) ---------- + struct _ForcePWM { + float force; + float pwm; + }; + + // Local variables used inside the interpolation + std::vector<_ForcePWM> _datatableForForceAndPWM; + + std::vector<_ForcePWM> _loadDataFromCSV(const std::string& filepath) { + // Prepare the datastructure we will load our data in + std::vector<_ForcePWM> data; + + // Try opening the file and if suceeded fill our datastructure with all the values in the .CSV file + std::ifstream file(filepath); + if (file.is_open()) { + // Variable for the currect line value in the .CSV file + std::string line; + + // Skip the header line + std::getline(file, line); + + // Read data line by line + while (std::getline(file, line)) { + // Temporarty variables to store data correctly ---------- + // Define temporary placeholders for variables we are extracting + std::string tempVar; + // Define data structure format we want our .CSV vlaues to be ordered in + _ForcePWM formatOfForcePWMDatastructure; + + // Data manipulation ---------- + // csvLineSplit variable converts "line" variable to a char stream of data that can further be used to split and filter out values we want + std::istringstream csvLineSplit(line); + // Extract Forces from "csvLineSplit" variable + std::getline(csvLineSplit, tempVar, '\t'); + formatOfForcePWMDatastructure.force = std::stof(tempVar); + // Convert grams into Newtons as we expect to get Forces in Newtons but the .CSV file calculates forsces in grams + formatOfForcePWMDatastructure.force = formatOfForcePWMDatastructure.force * (9.81/1000.0); + // Extract PWM from "csvLineSplit" variable + std::getline(csvLineSplit, tempVar, '\t'); + formatOfForcePWMDatastructure.pwm = std::stof(tempVar); + + // Push processed data with correct formating into the complete dataset ---------- + data.push_back(formatOfForcePWMDatastructure); + } + + file.close(); + } else { + std::cerr << "ERROR: Unable to open file: " << filepath << std::endl; + exit(1); + } + + return data; } - file.close(); - } else { - std::cerr << "ERROR: Unable to open file: " << filepath << std::endl; - exit(1); - } + int16_t* _interpolate_force_to_pwm(float* forces) { + int16_t* interpolatedPWMArray = new int16_t[4]{0, 0, 0, 0}; + + // Interpolate ---------- + for (int8_t i = 0; i < 4; i++) { + // Edge case, if the force is out of the bounds of your data table, handle accordingly + if (forces[i] <= _datatableForForceAndPWM.front().force) { + interpolatedPWMArray[i] = static_cast(_datatableForForceAndPWM.front().pwm); // To small Force + } + else if (forces[i] >= _datatableForForceAndPWM.back().force) { + interpolatedPWMArray[i] = static_cast(_datatableForForceAndPWM.back().pwm); // To big Force + } + else { + // Set temporary variables for interpolating + // Initialize with the first element + _ForcePWM low = _datatableForForceAndPWM.front(); + _ForcePWM high; + + // Interpolate + // Find the two points surrounding the given force + // Run the loop until the force value we are givven is lower than the current dataset value + for (const _ForcePWM& CurrentForcePWMData : _datatableForForceAndPWM) { + if (CurrentForcePWMData.force >= forces[i]) { + high = CurrentForcePWMData; + break; + } + low = CurrentForcePWMData; + } + + // Linear interpolation formula: y = y0 + (y1 - y0) * ((x - x0) / (x1 - x0)) + float interpolatedPWM = low.pwm + (high.pwm - low.pwm) * ((forces[i] - low.force) / (high.force - low.force)); + + // Save the interpolated value in the final array + interpolatedPWMArray[i] = static_cast(interpolatedPWM); + } + } - return data; -} + return interpolatedPWMArray; + } + // Interpolation from forces to pwm functions (STOP) ---------- + + // The most important function that sends PWM values through I2C to Arduino that then sends PWM signal further to the ESCs + const int8_t _I2C_BUS = 1; + const int16_t _I2C_ADDRESS = 0x21; + const char* _I2C_DEVICE = "/dev/i2c-1"; + void _send_pwm_to_ESCs(int16_t* pwm) { + // Variables ---------- + // 4 PWM values of 16-bits + // Each byte is 8-bits + // 4*16-bits = 64-bits => 8 bytes + uint8_t dataSize = 8; + uint8_t messageInBytesPWM[8]; + + + // Convert PWM int 16-bit to bytes ---------- + for (int8_t i = 0; i < 4; i++) { + // Split the integer into most significant byte (MSB) and least significant byte (LSB) + uint8_t msb = (pwm[i] >> 8) & 0xFF; + uint8_t lsb = pwm[i] & 0xFF; + + // Add the bytes to the finished array + messageInBytesPWM[i*2] = msb; + messageInBytesPWM[i*2 + 1] = lsb; + } + + // Data sending ---------- + // Open I2C conection + int fileI2C = open(_I2C_DEVICE, O_RDWR); -int16_t *_interpolate_force_to_pwm(float *forces) { - int16_t *interpolatedPWMArray = new int16_t[4]{0, 0, 0, 0}; - - // Interpolate ---------- - for (int8_t i = 0; i < 4; i++) { - // Edge case, if the force is out of the bounds of your data table, handle - // accordingly - if (forces[i] <= _datatableForForceAndPWM.front().force) { - interpolatedPWMArray[i] = static_cast( - _datatableForForceAndPWM.front().pwm); // To small Force - } else if (forces[i] >= _datatableForForceAndPWM.back().force) { - interpolatedPWMArray[i] = static_cast( - _datatableForForceAndPWM.back().pwm); // To big Force - } else { - // Set temporary variables for interpolating - // Initialize with the first element - _ForcePWM low = _datatableForForceAndPWM.front(); - _ForcePWM high; - - // Interpolate - // Find the two points surrounding the given force - // Run the loop until the force value we are givven is lower than the - // current dataset value - for (const _ForcePWM &CurrentForcePWMData : _datatableForForceAndPWM) { - if (CurrentForcePWMData.force >= forces[i]) { - high = CurrentForcePWMData; - break; + // Error handling in case of edge cases with I2C + if (fileI2C < 0) { + std::cerr << "ERROR: Couldn't opening I2C device" << std::endl; + exit(2); + } + if (ioctl(fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { + std::cerr << "ERROR: Couldn't set I2C address" << std::endl; + close(fileI2C); // Close I2C connection before exiting + exit(3); } - low = CurrentForcePWMData; - } - // Linear interpolation formula: y = y0 + (y1 - y0) * ((x - x0) / (x1 - - // x0)) - float interpolatedPWM = - low.pwm + (high.pwm - low.pwm) * - ((forces[i] - low.force) / (high.force - low.force)); + // Send the I2C message + if (write(fileI2C, messageInBytesPWM, dataSize) != dataSize) { + std::cerr << "ERROR: Couldn't send data, ignoring message..." << std::endl; + } - // Save the interpolated value in the final array - interpolatedPWMArray[i] = static_cast(interpolatedPWM); + // Close I2C connection + close(fileI2C); + } + + // Initial function to set everything up with thruster driver + // Set initial PWM limiting variables + int8_t _thrusterMapping[4] = {0, 1, 2, 3}; + int8_t _thrusterDirection[4] = {1, 1, 1, 1}; + int16_t _offsetPWM[4] = {0, 0, 0, 0}; + int16_t _minPWM[4] = {1100, 1100, 1100, 1100}; + int16_t _maxPWM[4] = {1900, 1900, 1900, 1900}; + + void init(const std::string& pathToCSVFile, int8_t* thrusterMapping, int8_t* thrusterDirection, int16_t* offsetPWM, int16_t* minPWM, int16_t* maxPWM) { + // Load data from the .CSV file + // We load it here instead of interpolation step as this will save time as we only open and load all the data once, savind time in intrepolation isnce we dont need to open the .CSV file over and over again. + _datatableForForceAndPWM = _loadDataFromCSV(pathToCSVFile); + + // Set correct parameters + // - Thruster Mapping + // - Thruster Direction + // - Offset PWM + // - Limit PWM + for (int8_t i = 0; i < 4; i++) { + _thrusterMapping[i] = thrusterMapping[i]; + _thrusterDirection[i] = thrusterDirection[i]; + _offsetPWM[i] = offsetPWM[i]; + _minPWM[i] = minPWM[i]; + _maxPWM[i] = maxPWM[i]; + } } - } - return interpolatedPWMArray; -} -// Interpolation from forces to pwm functions (STOP) ---------- - -// The most important function that sends PWM values through I2C to Arduino that -// then sends PWM signal further to the ESCs -const int8_t _I2C_BUS = 1; -const int16_t _I2C_ADDRESS = 0x21; -const char *_I2C_DEVICE = "/dev/i2c-1"; -void _send_pwm_to_ESCs(int16_t *pwm) { - // Variables ---------- - // 4 PWM values of 16-bits - // Each byte is 8-bits - // 4*16-bits = 64-bits => 8 bytes - uint8_t dataSize = 8; - uint8_t messageInBytesPWM[8]; - - // Convert PWM int 16-bit to bytes ---------- - for (int8_t i = 0; i < 4; i++) { - // Split the integer into most significant byte (MSB) and least significant - // byte (LSB) - uint8_t msb = (pwm[i] >> 8) & 0xFF; - uint8_t lsb = pwm[i] & 0xFF; - - // Add the bytes to the finished array - messageInBytesPWM[i * 2] = msb; - messageInBytesPWM[i * 2 + 1] = lsb; - } - - // Data sending ---------- - // Open I2C conection - int fileI2C = open(_I2C_DEVICE, O_RDWR); - - // Error handling in case of edge cases with I2C - if (fileI2C < 0) { - std::cerr << "ERROR: Couldn't opening I2C device" << std::endl; - exit(2); - } - if (ioctl(fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { - std::cerr << "ERROR: Couldn't set I2C address" << std::endl; - close(fileI2C); // Close I2C connection before exiting - exit(3); - } - - // Send the I2C message - if (write(fileI2C, messageInBytesPWM, dataSize) != dataSize) { - std::cerr << "ERROR: Couldn't send data, ignoring message..." << std::endl; - exit(4); - } - - // Close I2C connection - close(fileI2C); -} + // The main core functionality of interacting and controling the thrusters + int16_t* drive_thrusters(float* thrusterForces) { + // Remap Thrusters + // From: [pin1:thruster1 , pin2:thruster2 , pin3:thruster3 , pin4:thruster4 ] + // To: [pin1:, pin2:, pin3:, pin4:] + float thrusterForcesCahngedMapping[4] = {0.0, 0.0, 0.0, 0.0}; + for (int8_t pinNr = 0; pinNr < 4; pinNr++) { + int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr]; + thrusterForcesCahngedMapping[pinNr] = thrusterForces[remapedThrusterForcesIndex]; + } -// Initial function to set everything up with thruster driver -// Set initial PWM limiting variables -int16_t _minPWM[4] = {1100, 1100, 1100, 1100}; -int16_t _maxPWM[4] = {1900, 1900, 1900, 1900}; - -void init(const std::string &pathToCSVFile, int16_t *minPWM, int16_t *maxPWM) { - // Load data from the .CSV file - // We load it here instead of interpolation step as this will save time as we - // only open and load all the data once, savind time in intrepolation isnce we - // dont need to open the .CSV file over and over again. - _datatableForForceAndPWM = _loadDataFromCSV(pathToCSVFile); - - // Set correct parameters for limiting PWM - for (int8_t i = 0; i < 4; i++) { - _minPWM[i] = minPWM[i]; - _maxPWM[i] = maxPWM[i]; - } -} + // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to the direction parameter + float thrusterForcesCahngedDirection[4] = {0.0, 0.0, 0.0, 0.0}; + for (int8_t i = 0; i < 4; i++) { + thrusterForcesCahngedDirection[i] = thrusterForcesCahngedMapping[i] * _thrusterDirection[i]; + } -// The main core functionality of interacting and controling the thrusters -int16_t *drive_thrusters(float *forces) { - // Interolate forces to raw PWM values - int16_t *pwm = _interpolate_force_to_pwm(forces); - - // Limit PWM - for (int8_t i = 0; i < 4; i++) { - if (pwm[i] < _minPWM[i]) { - pwm[i] = _minPWM[i]; // To small PWM - } else if (pwm[i] > _maxPWM[i]) { - pwm[i] = _maxPWM[i]; // To big PWM - } - } + // Interpolate forces to raw PWM values + int16_t* pwm = _interpolate_force_to_pwm(thrusterForcesCahngedDirection); + + // Offset PWM + for (int8_t i = 0; i < 4; i++) { + pwm[i] += _offsetPWM[i]; + } + + // Limit PWM + for (int8_t i = 0; i < 4; i++) { + if (pwm[i] < _minPWM[i]) { + pwm[i] = _minPWM[i]; // To small PWM + } + else if (pwm[i] > _maxPWM[i]) { + pwm[i] = _maxPWM[i]; // To big PWM + } + } - // Send PWM vlaues through I2C to the microcontroller to control thrusters - _send_pwm_to_ESCs(pwm); + // Send PWM vlaues through I2C to the microcontroller to control thrusters + _send_pwm_to_ESCs(pwm); - // Return PWM values for debuging and logging purposes - return pwm; + // Return PWM values for debuging and logging purposes + return pwm; + } } -} // namespace thruster_interface_asv_driver_lib + + diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp index 0285b754..20319e4d 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp @@ -2,88 +2,129 @@ #include class ThrusterInterfaceASVNode : public rclcpp::Node { -private: - // ROS2 Variables ---------- - // Creates ROS2 subscriber - rclcpp::Subscription::SharedPtr - _subscriberThrusterForces; - // Create ROS2 publisher - rclcpp::Publisher::SharedPtr _publisherPWM; - // Create ROS2 timer/cycler - rclcpp::TimerBase::SharedPtr _thruster_driver_timer; - - // Variables that are shared inside the object ---------- - // Mutable allows modification in const methods - mutable float _thrusterForces[4]; - mutable int16_t *_pwm; - - // Methods ---------- - void _thruster_forces_callback( - const std_msgs::msg::Float32MultiArray::SharedPtr msg) const { - // Save subscribed topic values into universal array for further usage - for (size_t i = 0; i < 4; i++) { - _thrusterForces[i] = msg->data[i]; + private: + // ROS2 Variables ---------- + // Creates ROS2 subscriber + rclcpp::Subscription::SharedPtr _subscriberThrusterForces; + // Create ROS2 publisher + rclcpp::Publisher::SharedPtr _publisherPWM; + // Create ROS2 timer/cycler + rclcpp::TimerBase::SharedPtr _thruster_driver_timer; + + // Variables that are shared inside the object ---------- + // Mutable allows modification in const methods + mutable float _thrusterForces[4]; + mutable int16_t* _pwm; + + // Methods ---------- + void _thruster_forces_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) const { + // Save subscribed topic values into universal array for further usage + for (size_t i = 0; i < 4; i++) { + _thrusterForces[i] = msg->data[i]; + } } - } - - void _thruster_driver_callback() { - // Drive thrusters according to the thrusterForces provided by subscriber - _pwm = thruster_interface_asv_driver_lib::drive_thrusters(_thrusterForces); - - // Publish PWM values for debuging purposes - std::shared_ptr messagePWM = - std::make_shared(); - messagePWM->data.resize(4); - for (int8_t i = 0; i < 4; i++) { - messagePWM->data[i] = _pwm[i]; + + void _thruster_driver_callback() { + // Drive thrusters according to the thrusterForces provided by subscriber + _pwm = thruster_interface_asv_driver_lib::drive_thrusters(_thrusterForces); + + // Publish PWM values for debuging purposes + std::shared_ptr messagePWM = std::make_shared(); + messagePWM->data.resize(4); + for (int8_t i = 0; i < 4; i++) { + messagePWM->data[i] = _pwm[i]; + } + _publisherPWM->publish(*messagePWM); + } + public: + // Builder for the object ---------- + ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") { + // Thruster Driver Setup ---------- + // Get filepath of .CSV file with ROS2 file path finder + std::string forcesToPWMDataFilePath = ament_index_cpp::get_package_share_directory("thruster_interface_asv"); + forcesToPWMDataFilePath += "/config/ThrustMe_P1000_force_mapping.csv"; + + // Get Thruster mapping values from ROS2 Parameters + std::vector parameterThrusterMapping = this->declare_parameter>("propulsion.thrusters.thruster_to_pin_mapping", {0, 1, 2, 3}); + int8_t ThrusterMapping[4] = { + static_cast(parameterThrusterMapping[0]), + static_cast(parameterThrusterMapping[1]), + static_cast(parameterThrusterMapping[2]), + static_cast(parameterThrusterMapping[3]) + }; + + // Get Thruster direction values from ROS2 Parameters + std::vector parameterThrusterDirection = this->declare_parameter>("propulsion.thrusters.thruster_direction", {1, 1, 1, 1}); + int8_t thrusterDirection[4] = { + static_cast(parameterThrusterDirection[0]), + static_cast(parameterThrusterDirection[1]), + static_cast(parameterThrusterDirection[2]), + static_cast(parameterThrusterDirection[3]) + }; + + // Get PWM Offset values from ROS2 Parameters + std::vector parameterPWMOffset = this->declare_parameter>("propulsion.thrusters.thruster_PWM_offset", {0, 0, 0, 0}); + int16_t offsetPWM[4] = { + static_cast(parameterPWMOffset[0]), + static_cast(parameterPWMOffset[1]), + static_cast(parameterPWMOffset[2]), + static_cast(parameterPWMOffset[3]) + }; + + // Get MAX and MIN PWM values from ROS2 Parameters + std::vector parameterPWMMin = this->declare_parameter>("propulsion.thrusters.thruster_PWM_min", {1100, 1100, 1100, 1100}); + std::vector parameterPWMMax = this->declare_parameter>("propulsion.thrusters.thruster_PWM_max", {1900, 1900, 1900, 1900}); + int16_t minPWM[4] = { + static_cast(parameterPWMMin[0]), + static_cast(parameterPWMMin[1]), + static_cast(parameterPWMMin[2]), + static_cast(parameterPWMMin[3]) + }; + int16_t maxPWM[4] = { + static_cast(parameterPWMMax[0]), + static_cast(parameterPWMMax[1]), + static_cast(parameterPWMMax[2]), + static_cast(parameterPWMMax[3]) + }; + + // Initialize Thruster driver + thruster_interface_asv_driver_lib::init( + forcesToPWMDataFilePath, + ThrusterMapping, + thrusterDirection, + offsetPWM, + minPWM, + maxPWM + ); + + // ROS Setup ---------- + // Initialize ROS2 subscribers role + _subscriberThrusterForces = this->create_subscription( + "/thrust/thruster_forces", + 5, + std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, this, std::placeholders::_1) + ); + + // Initialize ROS2 publisher role + _publisherPWM = this->create_publisher( + "/pwm", + 5 + ); + + // Initialize a never ending cycle that continuisly publishes and drives thrusters depending on what the ThrusterForces value is set to from ThrusterForces subscriber + using namespace std::chrono_literals; + _thruster_driver_timer = this->create_wall_timer( + 200ms, + std::bind(&ThrusterInterfaceASVNode::_thruster_driver_callback, this) + ); + + // Debugging ---------- + RCLCPP_INFO(this->get_logger(), "Initialized thruster_interface_asv_node node"); } - _publisherPWM->publish(*messagePWM); - } - -public: - // Builder for the object ---------- - ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") { - // Thruster Driver Setup ---------- - // Get MAX and MIN PWM values - int16_t minPWM[4] = {1100, 1100, 1100, 1100}; - int16_t maxPWM[4] = {1900, 1900, 1900, 1900}; - - // Get filepath of .CSV file with ROS2 file path finder - std::string forcesToPWMDataFilePath = - ament_index_cpp::get_package_share_directory("thruster_interface_asv"); - forcesToPWMDataFilePath += "/config/ThrustMe_P1000_force_mapping.csv"; - - // Initialize Thruster driver - thruster_interface_asv_driver_lib::init(forcesToPWMDataFilePath, minPWM, - maxPWM); - - // ROS Setup ---------- - // Initialize ROS2 subscribers role - _subscriberThrusterForces = - this->create_subscription( - "/thrust/thruster_forces", 5, - std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, - this, std::placeholders::_1)); - - // Initialize ROS2 publisher role - _publisherPWM = - this->create_publisher("/pwm", 5); - - // Initialize a never ending cycle that continuisly publishes and drives - // thrusters depending on what the ThrusterForces value is set to from - // ThrusterForces subscriber - using namespace std::chrono_literals; - _thruster_driver_timer = this->create_wall_timer( - 500ms, - std::bind(&ThrusterInterfaceASVNode::_thruster_driver_callback, this)); - - // Debugging ---------- - RCLCPP_INFO(this->get_logger(), - "Initialized thruster_interface_asv_node node"); - } }; -int main(int argc, char *argv[]) { +int main(int argc, char * argv[]) +{ // Start ROS2 rclcpp::init(argc, argv);