diff --git a/motion/thruster_interface_asv/CMakeLists.txt b/motion/thruster_interface_asv/CMakeLists.txt index 39a08ae..12734a7 100644 --- a/motion/thruster_interface_asv/CMakeLists.txt +++ b/motion/thruster_interface_asv/CMakeLists.txt @@ -2,29 +2,32 @@ cmake_minimum_required(VERSION 3.8) project(thruster_interface_asv) if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + set(CMAKE_CXX_STANDARD 20) endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(ament_index_cpp REQUIRED) +find_package(vortex_msgs REQUIRED) +find_package(fmt REQUIRED) include_directories(include) add_executable(${PROJECT_NAME}_node - src/thruster_interface_asv_driver_lib.cpp src/thruster_interface_asv_node.cpp + src/thruster_interface_asv_ros.cpp + src/thruster_interface_asv_driver.cpp ) ament_target_dependencies(${PROJECT_NAME}_node rclcpp std_msgs - ament_index_cpp + vortex_msgs +) + +target_link_libraries(${PROJECT_NAME}_node + fmt::fmt + spdlog::spdlog ) install(DIRECTORY diff --git a/motion/thruster_interface_asv/README.md b/motion/thruster_interface_asv/README.md new file mode 100644 index 0000000..823dd7d --- /dev/null +++ b/motion/thruster_interface_asv/README.md @@ -0,0 +1,41 @@ +# Thruster Interface ASV + +This package provides an interface for controlling the thrusters of freya converting forces values to pwm values. Similarly as what done with orca (see https://github.com/vortexntnu/vortex-auv/tree/main/motion/thruster_interface_auv) the mapping is based on a piecewise third order polynomial approximating the datasheet .csv table found in /resources. Values send via i2c protocol. Below it's shown how to derive the coefficients from the .csv table provided in /resources. + +The analysis covers the table split in two halves: +1) values > 0 + the leftmost green 0-valued point for a better fitting (see picture below) +2) values < 0 with the rightmost red 0-valued point + +![fitting](resources/poly_approx_image.png) + +## Usage + +```sh +source install/setup.bash +ros2 launch thruster_interface_asv thruster_interface_asv.launch.py +``` + +## Structure + +### Nodes + +1. `thruster_interface_asv_node.cpp` contains the main loop of the node + +2. `thruster_interface_asv_ros.cpp` contains the implementation of the node dependent on ros2 creating a subscriber for thruster forces, a publisher for pwm values, and a driver for handling the mapping. Initialize everything extracting all the parameters from .yaml file found in `../asv_setup/config/robots/freya.yaml` and `/config/thruster_interface_asv_config.yaml`. + +3. `thruster_interface_asv_driver.cpp` contains the pure .cpp implementation for the mapping, conversion for i2c data format, and sending those values. + +### Topics + +- *subscribe to:* `/freya/thruster_forces [std_msgs/msg/float64_multi_array]` -- array of forces to apply to each thruster. + +- *publish:* `/freya/pwm_output [std_msgs/msg/Int16MultiArray]` -- pwm command value to apply that force. + +## Config + +1. Edit `../asv_setup/config/robots/freya.yaml` for thruster parameters like mapping, direction, pwm_min and max. +2. Edit `/config/thruster_interface_asv_config.yaml` for i2c bus and address, and polynomial coefficients for the mapping. + +## Contact + +For questions or support, please contact [albertomorselli00@gmail.com](mailto:albertomorselli00@gmail.com). diff --git a/motion/thruster_interface_asv/config/thruster_interface_asv_config.yaml b/motion/thruster_interface_asv/config/thruster_interface_asv_config.yaml new file mode 100644 index 0000000..dfc607d --- /dev/null +++ b/motion/thruster_interface_asv/config/thruster_interface_asv_config.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + coeffs: + LEFT: [0.05127, -1.33210, 3.51027, 1479.85269] + RIGHT: [0.04171, 0.67438, 2.13177, 1521.64815] + + i2c: + bus: 1 + address: 0x21 + + debug: + flag: False diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver.hpp new file mode 100644 index 0000000..b8a644b --- /dev/null +++ b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver.hpp @@ -0,0 +1,160 @@ +#ifndef THRUSTER_INTERFACE_ASV_DRIVER_HPP +#define THRUSTER_INTERFACE_ASV_DRIVER_HPP + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief struct to hold the parameters for a single thruster + */ +struct ThrusterParameters { + uint8_t mapping; + int8_t direction; + uint16_t pwm_min; + uint16_t pwm_max; +}; + +enum PolySide { + LEFT = 0, + RIGHT = 1 +}; // vector index for the position of the coefficients in the coeff vector + +/** + * @brief class instantiated by ThrusterInterfaceASVNode to control the + * thrusters, takes the thruster forces and converts them to PWM signals to be + * sent via I2C to the ESCs (PCA9685 Adafruit 16-Channel 12-bit PWM/Servo + * Driver) + * + * @details Based on the datasheets found in /resources, approximate the map + * with a piecewise (>0 and <0) third order polynomial. + * + * @note The coefficients are found in the config.yaml for all the possible + * SYSTEM.OPERATIONAL_VOLTAGE values, but we use only 16V for now, so: removed + * all the handling of the other voltages to save resources. Could be + * re-implemented in the future for more flexibility if we ever need it to + * operate at different voltages in different situations. + */ +class ThrusterInterfaceASVDriver { + public: + ~ThrusterInterfaceASVDriver(); + + /** + * @brief called from ThrusterInterfaceASVNode .cpp when instantiating the + * object, initializes all the params. + * + * @param i2c_bus bus number used to communicate + * @param pico_i2c_address i2c address of the ESC that drive the + * @param thruster_parameters describe mapping, direction, min and max pwm + * value for each thruster + * @param poly_coeffs LEFT(<0) and RIGHT(>0) third order + * polynomial coefficients + */ + ThrusterInterfaceASVDriver( + short i2c_bus, + int pico_i2c_address, + const std::vector& thruster_parameters, + const std::vector>& poly_coeffs); + /** + * @brief calls both 1) interpolate_forces_to_pwm() to + * convert the thruster forces to PWM values and 2) send_data_to_escs() to + * send them to the ESCs via I2C + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster + */ + std::vector drive_thrusters( + const std::vector& thruster_forces_array); + + private: + int bus_fd_; ///< file descriptor for the I2C bus (integer >0 that uniquely + ///< identifies the device. -1 if it fails) + + int i2c_bus_; + int pico_i2c_address_; + std::vector thruster_parameters_; + std::vector> poly_coeffs_; + + uint16_t idle_pwm_value_; ///< pwm value when force = 0.00 + + /** + * @brief only take the thruster forces and return PWM values + * + * @param thruster_forces_array vector of forces for each thruster + * + * @return std::vector vector of pwm values sent to each thruster + * if we want to publish them for debug purposes + */ + std::vector interpolate_forces_to_pwm( + const std::vector& thruster_forces_array); + + /** + * @brief scalar map from force to pwm x->y. Choose coefficients [LEFT] or + * [RIGHT] based on sign(force) + * + * @param force scalar force value + * @param coeffs std::vector> coeffs contains the pair + * of coefficients + * + * @return std::uint16_t scalar pwm value + */ + std::uint16_t force_to_pwm(double force, + const std::vector>& coeffs); + + /** + * @brief compute y = a*x^3 + b*x^2 + c*x + d + * @param force x + * @param coeffs a,b,c,d + * + * @return std::uint16_t pwm value + */ + std::uint16_t calc_poly(double force, const std::vector& coeffs); + + /** + * @brief only takes the pwm values computed and sends them + * to the ESCs via I2C + * + * @param thruster_pwm_array vector of pwm values to send + */ + void send_data_to_escs(const std::vector& thruster_pwm_array); + + /** + * @brief convert Newtons to Kg + * + * @param force Newtons + * + * @return double Kg + */ + static constexpr double to_kg(double force) { return force / 9.80665; } + + /** + * @brief convert pwm values to i2c bytes + * + * @param pwm pwm value + * + * @return std::array i2c data + */ + static constexpr std::array pwm_to_i2c_data( + std::uint16_t pwm) { + return {static_cast((pwm >> 8) & 0xFF), + static_cast(pwm & 0xFF)}; + } +}; + +#endif // THRUSTER_INTERFACE_ASV_DRIVER_HPP 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 deleted file mode 100644 index 5420c24..0000000 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_driver_lib.hpp +++ /dev/null @@ -1,23 +0,0 @@ -// C++ Libraries -#include -#include -#include -#include -#include -#include -#include - -#include // Used for the O_RDWR define -#include // Used for the I2C_SLAVE define -#include -#include // Used for the close function - -namespace thruster_interface_asv_driver_lib { -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); -} // namespace thruster_interface_asv_driver_lib 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 deleted file mode 100644 index 61cc52a..0000000 --- a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_node.hpp +++ /dev/null @@ -1,12 +0,0 @@ -// C++ Libraries -#include - -// ROS2 libraries -#include -#include // For the ROS2 Timer clock -#include -#include -#include - -// Custom libraries -#include diff --git a/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_ros.hpp b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_ros.hpp new file mode 100644 index 0000000..916a473 --- /dev/null +++ b/motion/thruster_interface_asv/include/thruster_interface_asv/thruster_interface_asv_ros.hpp @@ -0,0 +1,84 @@ +#ifndef THRUSTER_INTERFACE_ASV_NODE_HPP +#define THRUSTER_INTERFACE_ASV_NODE_HPP + +#include +#include +#include +#include +#include +#include "thruster_interface_asv/thruster_interface_asv_driver.hpp" + +class ThrusterInterfaceASVNode : public rclcpp::Node { + public: + ThrusterInterfaceASVNode(); + + private: + /** + * @brief periodically receive thruster forces topic + * + * @param msg ThrusterForces message + */ + void thruster_forces_callback( + const std_msgs::msg::Float64MultiArray::SharedPtr msg); + + /** + * @brief publish and send pwm commands to thrusters. Sinchronous with + * thruster_forces_callback + */ + void pwm_callback(); + + /** + * @brief extract all parameters from the .yaml file + */ + void extract_all_parameters(); + + /** + * @brief Initialize the parameter handler and a parameter event callback. + * + */ + void initialize_parameter_handler(); + + /** + * @brief create/reinitialize publisher if flag=1, kill it if flag=0 + */ + void set_publisher(); + + int i2c_bus_; + int i2c_address_; + std::string subscriber_topic_name_; + std::string publisher_topic_name_; + std::vector thruster_parameters_; + std::vector> poly_coeffs_; + + std::vector thruster_forces_array_; + bool debug_flag_; + + std::unique_ptr thruster_driver_; + rclcpp::Subscription::SharedPtr + thruster_forces_subscriber_; + rclcpp::Publisher::SharedPtr + thruster_pwm_publisher_; + + /** + * @brief Manages parameter events for the node. + * + * This handler is used to set up a mechanism to listen for and react to + * changes in parameters via terminal at runtime. + */ + std::shared_ptr param_handler_; + + /** + * @brief Handle to the registration of the parameter event callback. + * + * Represents a token or reference to the specific callback registration + * made with the parameter event handler (`param_handler_`). + */ + rclcpp::ParameterCallbackHandle::SharedPtr debug_flag_parameter_cb; + + /** + * specific callback for updating debug_flag. + */ + void update_debug_flag(const rclcpp::Parameter& p); +}; + +#endif // THRUSTER_INTERFACE_ASV_NODE_HPP diff --git a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py index 317e09f..bde860a 100755 --- a/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py +++ b/motion/thruster_interface_asv/launch/thruster_interface_asv.launch.py @@ -5,19 +5,28 @@ from launch_ros.actions import Node -def generate_launch_description(): - # Path to the YAML file - config = path.join( - get_package_share_directory("asv_setup"), 'config', 'robots', "freya.yaml" - ) +def generate_launch_description() -> LaunchDescription: + config = [ + path.join( + get_package_share_directory(package_name="asv_setup"), + "config", + "robots", + "freya.yaml", + ), + path.join( + get_package_share_directory(package_name="thruster_interface_asv"), + "config", + "thruster_interface_asv_config.yaml", + ), + ] thruster_interface_asv_node = Node( - package='thruster_interface_asv', - executable='thruster_interface_asv_node', - name='thruster_interface_asv_node', - namespace='freya', - output='screen', - parameters=[config], + package="thruster_interface_asv", + executable="thruster_interface_asv_node", + name="thruster_interface_asv_node", + namespace="freya", + output="screen", + parameters=config, ) return LaunchDescription([thruster_interface_asv_node]) diff --git a/motion/thruster_interface_asv/package.xml b/motion/thruster_interface_asv/package.xml index 6f87683..079043b 100644 --- a/motion/thruster_interface_asv/package.xml +++ b/motion/thruster_interface_asv/package.xml @@ -2,16 +2,16 @@ thruster_interface_asv - 0.0.0 - Interface for transforming thrust to thruster rpm - alekskl01 + 1.0.0 + Thruster interface to control ASV thrusters through PCA9685 Module via i2c, mapping force values to pwm values + vortex MIT ament_cmake rclcpp std_msgs - ament_index_cpp + vortex_msgs ament_cmake diff --git a/motion/thruster_interface_asv/resources/README.md b/motion/thruster_interface_asv/resources/README.md new file mode 100644 index 0000000..5f051ab --- /dev/null +++ b/motion/thruster_interface_asv/resources/README.md @@ -0,0 +1 @@ +*PLEASE NOTE: values are in expressed in grams in the table here. They have been converted to kg before fitting the coefficients. Coefficients in .yaml file are valid for pwm/kg* diff --git a/motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv b/motion/thruster_interface_asv/resources/ThrustMe_P1000_force_mapping.csv similarity index 100% rename from motion/thruster_interface_asv/config/ThrustMe_P1000_force_mapping.csv rename to motion/thruster_interface_asv/resources/ThrustMe_P1000_force_mapping.csv diff --git a/motion/thruster_interface_asv/resources/poly_approx_image.png b/motion/thruster_interface_asv/resources/poly_approx_image.png new file mode 100644 index 0000000..ca572bc Binary files /dev/null and b/motion/thruster_interface_asv/resources/poly_approx_image.png differ diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_driver.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_driver.cpp new file mode 100644 index 0000000..026f270 --- /dev/null +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_driver.cpp @@ -0,0 +1,132 @@ +#include "thruster_interface_asv/thruster_interface_asv_driver.hpp" + +ThrusterInterfaceASVDriver::ThrusterInterfaceASVDriver( + short i2c_bus, + int pico_i2c_address, + const std::vector& thruster_parameters, + const std::vector>& poly_coeffs) + : i2c_bus_(i2c_bus), + pico_i2c_address_(pico_i2c_address), + thruster_parameters_(thruster_parameters), + poly_coeffs_(poly_coeffs) { + std::string i2c_filename = "/dev/i2c-" + std::to_string(i2c_bus_); + bus_fd_ = + open(i2c_filename.c_str(), + O_RDWR); // Open the i2c bus for reading and writing (0_RDWR) + if (bus_fd_ < 0) { + throw std::runtime_error( + fmt::format("ERROR: Failed to open I2C bus {} : {}", i2c_bus_, + strerror(errno))); + return; + } + + idle_pwm_value_ = + (calc_poly(0, poly_coeffs_[LEFT]) + calc_poly(0, poly_coeffs_[RIGHT])) / + 2; +} + +ThrusterInterfaceASVDriver::~ThrusterInterfaceASVDriver() { + if (bus_fd_ >= 0) { + send_data_to_escs(std::vector(thruster_parameters_.size(), + idle_pwm_value_)); + close(bus_fd_); + } +} + +std::vector ThrusterInterfaceASVDriver::interpolate_forces_to_pwm( + const std::vector& thruster_forces_array) { + // Convert Newtons to Kg (since the thruster datasheet is in Kg) + std::vector forces_in_kg(thruster_forces_array.size()); + std::transform(thruster_forces_array.begin(), thruster_forces_array.end(), + forces_in_kg.begin(), to_kg); + + std::vector interpolated_pwm; + for (const double force : forces_in_kg) { + interpolated_pwm.push_back(force_to_pwm(force, poly_coeffs_)); + } + return interpolated_pwm; +} + +std::uint16_t ThrusterInterfaceASVDriver::force_to_pwm( + double force, + const std::vector>& coeffs) { + if (force < 0) { + return calc_poly(force, coeffs[LEFT]); + } else if (force > 0) { + return calc_poly(force, coeffs[RIGHT]); + } else { + return idle_pwm_value_; // 1500 + } +} + +std::uint16_t ThrusterInterfaceASVDriver::calc_poly( + double force, + const std::vector& coeffs) { + return static_cast(coeffs[0] * std::pow(force, 3) + + coeffs[1] * std::pow(force, 2) + + coeffs[2] * force + coeffs[3]); +} + +void ThrusterInterfaceASVDriver::send_data_to_escs( + const std::vector& thruster_pwm_array) { + constexpr std::size_t i2c_data_size = + 1 + 4 * 2; // 4 thrusters * (1xMSB + 1xLSB) + std::vector i2c_data_array; + i2c_data_array.reserve(i2c_data_size); + + i2c_data_array.push_back(0x00); // Start byte + std::for_each(thruster_pwm_array.begin(), thruster_pwm_array.end(), + [&](std::uint16_t pwm) { + std::array bytes = pwm_to_i2c_data(pwm); + std::copy(bytes.begin(), bytes.end(), + std::back_inserter(i2c_data_array)); + }); + + // Set the I2C slave address + if (ioctl(bus_fd_, I2C_SLAVE, pico_i2c_address_) < 0) { + throw std::runtime_error(fmt::format("Failed to open I2C bus {} : {}", + i2c_bus_, strerror(errno))); + return; + } + + // Write data to the I2C device + if (write(bus_fd_, i2c_data_array.data(), i2c_data_size) != i2c_data_size) { + throw std::runtime_error(fmt::format( + "ERROR: Failed to write to I2C device : {}", strerror(errno))); + } +} + +std::vector ThrusterInterfaceASVDriver::drive_thrusters( + const std::vector& thruster_forces_array) { + // Apply thruster mapping and direction + std::vector mapped_forces(thruster_forces_array.size()); + for (size_t i = 0; i < thruster_parameters_.size(); ++i) { + mapped_forces[i] = + thruster_forces_array[thruster_parameters_[i].mapping] * + thruster_parameters_[i].direction; + } + + // Convert forces to PWM + std::vector thruster_pwm_array = + interpolate_forces_to_pwm(mapped_forces); + + // Apply thruster offset and limit PWM if needed + for (size_t i = 0; i < thruster_pwm_array.size(); ++i) { + // Clamp the PWM signal + if (thruster_pwm_array[i] < thruster_parameters_[i].pwm_min) { + thruster_pwm_array[i] = thruster_parameters_[i].pwm_min; + } else if (thruster_pwm_array[i] > thruster_parameters_[i].pwm_max) { + thruster_pwm_array[i] = thruster_parameters_[i].pwm_max; + } + } + + try { + send_data_to_escs(thruster_pwm_array); + } catch (const std::exception& e) { + spdlog::error("ERROR: Failed to send PWM values - {}", e.what()); + } catch (...) { + spdlog::error("ERROR: Failed to send PWM values - Unknown exception"); + } + + return thruster_pwm_array; +} 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 deleted file mode 100644 index c18263f..0000000 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_driver_lib.cpp +++ /dev/null @@ -1,239 +0,0 @@ -#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> _ForcePWMTable; - -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 succeeded fill our datastructure with all the - // values from the force mapping .CSV file - std::ifstream file(filepath); - if (file.is_open()) { - 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 values to be - // ordered in - _ForcePWM ForcePWMDataStructure; - - // 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'); - ForcePWMDataStructure.force = std::stof(tempVar); - - // Convert grams into Newtons as we expect to get Forces in Newtons - // but the .CSV file contains forces in grams - ForcePWMDataStructure.force = - ForcePWMDataStructure.force * (9.81 / 1000.0); - - // Extract PWM from "csvLineSplit" variable - std::getline(csvLineSplit, tempVar, '\t'); - ForcePWMDataStructure.pwm = std::stof(tempVar); - - // Push processed data with correct formatting into the complete - // dataset - // ---------- - data.push_back(ForcePWMDataStructure); - } - - file.close(); - } else { - std::cerr << "ERROR: Unable to open file: " << filepath << std::endl; - exit(1); - } - - return data; -} - -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] <= _ForcePWMTable.front().force) { - interpolatedPWMArray[i] = static_cast( - _ForcePWMTable.front().pwm); // Too small Force - - } else if (forces[i] >= _ForcePWMTable.back().force) { - interpolatedPWMArray[i] = static_cast( - _ForcePWMTable.back().pwm); // Too big Force - - } else { - // Set temporary variables for interpolating - // Initialize with the first element - _ForcePWM low = _ForcePWMTable.front(); - _ForcePWM high; - - // Interpolate - // Find the two points surrounding the given force - // Run the loop until the force value we are given is lower than - // the current dataset value - for (const _ForcePWM& CurrentForcePWMData : _ForcePWMTable) { - 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 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"; -int _fileI2C = -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 ---------- - try { - // Send the I2C message - if (write(_fileI2C, messageInBytesPWM, dataSize) != dataSize) { - throw std::runtime_error( - "ERROR: Couldn't send data, ignoring message..."); - } - } catch (const std::exception& error) { - std::cerr << error.what() << std::endl; - } -} - -// Initial function to set everything up with thruster driver -// Set initial PWM limiting variables -int8_t _thrusterMapping[4]; -int8_t _thrusterDirection[4]; -int16_t _offsetPWM[4]; -int16_t _minPWM[4]; -int16_t _maxPWM[4]; - -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 - _ForcePWMTable = _loadDataFromCSV(pathToCSVFile); - - // Set correct parameters - 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]; - } - - // Connecting to the I2C - try { - // Open I2C connection - _fileI2C = open(_I2C_DEVICE, O_RDWR); - - // Error handling in case of edge cases with I2C - if (_fileI2C < 0) { - throw std::runtime_error("ERROR: Couldn't opening I2C device"); - } - if (ioctl(_fileI2C, I2C_SLAVE, _I2C_ADDRESS) < 0) { - throw std::runtime_error("ERROR: Couldn't set I2C address"); - } - } catch (const std::exception& error) { - std::cerr << error.what() << std::endl; - } -} - -// The main core functionality of interacting and controlling the thrusters -int16_t* drive_thrusters(float* thrusterForces) { - // Change direction of the thruster (Forward(+1)/Backwards(-1)) according to - // the direction parameter - float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0}; - for (int8_t i = 0; i < 4; i++) { - thrusterForcesChangedDirection[i] = - thrusterForces[i] * _thrusterDirection[i]; - } - - // Remap Thrusters - // From: [pin1:thruster1, pin2:thruster2, pin3:thruster3, pin4:thruster4] - // To: [pin1:, pin2:, - // pin3:, pin4:] - float thrusterForcesChangedMapping[4] = {0.0, 0.0, 0.0, 0.0}; - for (int8_t pinNr = 0; pinNr < 4; pinNr++) { - int8_t remapedThrusterForcesIndex = _thrusterMapping[pinNr]; - thrusterForcesChangedMapping[pinNr] = - thrusterForcesChangedDirection[remapedThrusterForcesIndex]; - } - - // Interpolate forces to raw PWM values - int16_t* pwm = _interpolate_force_to_pwm(thrusterForcesChangedMapping); - - // Offset PWM - for (int8_t i = 0; i < 4; i++) { - pwm[i] += _offsetPWM[i]; - } - - // Limit PWM - for (int8_t i = 0; i < 4; i++) { - pwm[i] = std::clamp(pwm[i], _minPWM[i], _maxPWM[i]); - } - - // Send PWM values through I2C to the microcontroller to control thrusters - _send_pwm_to_ESCs(pwm); - - // Return PWM values for debugging 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 a86c224..b41797e 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp @@ -1,150 +1,9 @@ -// Custom libraries -#include +#include "thruster_interface_asv/thruster_interface_asv_ros.hpp" -class ThrusterInterfaceASVNode : public rclcpp::Node { - private: - // ROS2 Variables ---------- - rclcpp::Subscription::SharedPtr - _subscriberThrusterForces; - rclcpp::Publisher::SharedPtr _publisherPWM; - 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::Float64MultiArray::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 debugging 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 force mapping - 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 thrusterForces subscriber - - this->declare_parameter("topics.thruster_forces"); - this->declare_parameter("topics.pwm_output"); - std::string thruster_forces_topic = - this->get_parameter("topics.thruster_forces").as_string(); - std::string pwm_output_topic = - this->get_parameter("topics.pwm_output").as_string(); - - _subscriberThrusterForces = - this->create_subscription( - thruster_forces_topic, 5, - std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, - this, std::placeholders::_1)); - - // Initialize ROS2 pwm publisher - _publisherPWM = this->create_publisher( - pwm_output_topic, 5); - - // Initialize a never ending cycle that continuously 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"); - } -}; - -int main(int argc, char* argv[]) { +int main(int argc, char** argv) { rclcpp::init(argc, argv); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Started thruster_interface_asv_node"); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; diff --git a/motion/thruster_interface_asv/src/thruster_interface_asv_ros.cpp b/motion/thruster_interface_asv/src/thruster_interface_asv_ros.cpp new file mode 100644 index 0000000..edd28ae --- /dev/null +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_ros.cpp @@ -0,0 +1,142 @@ +#include "thruster_interface_asv/thruster_interface_asv_ros.hpp" + +ThrusterInterfaceASVNode::ThrusterInterfaceASVNode() + : Node("thruster_interface_asv_node") { + this->extract_all_parameters(); + + // Set up subscriber and publisher + rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data; + auto qos_sensor_data = rclcpp::QoS( + rclcpp::QoSInitialization(qos_profile.history, 1), qos_profile); + + thruster_forces_subscriber_ = + this->create_subscription( + subscriber_topic_name_, qos_sensor_data, + std::bind(&ThrusterInterfaceASVNode::thruster_forces_callback, this, + std::placeholders::_1)); + + thruster_pwm_publisher_ = + this->create_publisher( + publisher_topic_name_, qos_sensor_data); + + // call constructor for thruster_driver_ + thruster_driver_ = std::make_unique( + i2c_bus_, i2c_address_, thruster_parameters_, poly_coeffs_); + + // Declare thruster_forces_array_ in case no topic comes at the + // very beginning + thruster_forces_array_ = std::vector(4, 0.00); + + this->initialize_parameter_handler(); + + RCLCPP_INFO(this->get_logger(), + "\"thruster_interface_asv_node\" correctly initialized"); +} + +void ThrusterInterfaceASVNode::thruster_forces_callback( + const std_msgs::msg::Float64MultiArray::SharedPtr msg) { + std::copy_n(msg->data.begin(), msg->data.size(), + thruster_forces_array_.begin()); + this->pwm_callback(); +} + +void ThrusterInterfaceASVNode::pwm_callback() { + // drive thrusters... + std::vector thruster_pwm_array = + thruster_driver_->drive_thrusters(this->thruster_forces_array_); + + //..and publish PWM values for debugging purposes + if (debug_flag_) { + std_msgs::msg::Int16MultiArray pwm_message; + pwm_message.data = std::vector(thruster_pwm_array.begin(), + thruster_pwm_array.end()); + thruster_pwm_publisher_->publish(pwm_message); + } +} + +void ThrusterInterfaceASVNode::initialize_parameter_handler() { + param_handler_ = std::make_shared(this); + + // Register the parameter event callback directly in the lambda expression + debug_flag_parameter_cb = param_handler_->add_parameter_callback( + "debug.flag", std::bind(&ThrusterInterfaceASVNode::update_debug_flag, + this, std::placeholders::_1)); +} + +void ThrusterInterfaceASVNode::update_debug_flag(const rclcpp::Parameter& p) { + debug_flag_ = p.get_value(); + RCLCPP_INFO(this->get_logger(), + "Received parameter event: debug.flag updated to: %s", + debug_flag_ ? "true" : "false"); +} + +void ThrusterInterfaceASVNode::extract_all_parameters() { + // thruster parameters from orca.yaml + this->declare_parameter>( + "propulsion.thrusters.thruster_to_pin_mapping"); + this->declare_parameter>( + "propulsion.thrusters.thruster_direction"); + this->declare_parameter>( + "propulsion.thrusters.thruster_PWM_min"); + this->declare_parameter>( + "propulsion.thrusters.thruster_PWM_max"); + + this->declare_parameter>("coeffs.LEFT"); + this->declare_parameter>("coeffs.RIGHT"); + + this->declare_parameter("i2c.bus"); + this->declare_parameter("i2c.address"); + + this->declare_parameter("topics.thruster_forces"); + this->declare_parameter("topics.pwm_output"); + + this->declare_parameter("debug.flag"); + + //----------------------------------------------------------------------- + // get them + auto thruster_mapping = + this->get_parameter("propulsion.thrusters.thruster_to_pin_mapping") + .as_integer_array(); + auto thruster_direction = + this->get_parameter("propulsion.thrusters.thruster_direction") + .as_integer_array(); + auto thruster_PWM_min = + this->get_parameter("propulsion.thrusters.thruster_PWM_min") + .as_integer_array(); + auto thruster_PWM_max = + this->get_parameter("propulsion.thrusters.thruster_PWM_max") + .as_integer_array(); + + std::vector left_coeffs = + this->get_parameter("coeffs.LEFT").as_double_array(); + std::vector right_coeffs = + this->get_parameter("coeffs.RIGHT").as_double_array(); + + this->i2c_bus_ = this->get_parameter("i2c.bus").as_int(); + this->i2c_address_ = this->get_parameter("i2c.address").as_int(); + + this->subscriber_topic_name_ = + this->get_parameter("topics.thruster_forces").as_string(); + this->publisher_topic_name_ = + this->get_parameter("topics.pwm_output").as_string(); + + this->debug_flag_ = this->get_parameter("debug.flag").as_bool(); + + std::transform( + thruster_mapping.begin(), thruster_mapping.end(), + thruster_direction.begin(), + std::back_inserter(this->thruster_parameters_), + [&](const int64_t& mapping, const int64_t& direction) { + size_t index = + std::distance(thruster_mapping.begin(), + std::find(thruster_mapping.begin(), + thruster_mapping.end(), mapping)); + return ThrusterParameters{ + static_cast(mapping), static_cast(direction), + static_cast(thruster_PWM_min[index]), + static_cast(thruster_PWM_max[index])}; + }); + + this->poly_coeffs_.push_back(left_coeffs); + this->poly_coeffs_.push_back(right_coeffs); +}