Refactor thruster interface

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

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

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

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 <fcntl.h>
+#include <fmt/core.h>
+#include <linux/i2c-dev.h>
+#include <spdlog/spdlog.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <map>
+#include <ranges>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+/**
+ * @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<ThrusterParameters>& thruster_parameters,
+        const std::vector<std::vector<double>>& 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<uint16_t> vector of pwm values sent to each thruster
+     */
+    std::vector<uint16_t> drive_thrusters(
+        const std::vector<double>& 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<ThrusterParameters> thruster_parameters_;
+    std::vector<std::vector<double>> 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<uint16_t> vector of pwm values sent to each thruster
+     * if we want to publish them for debug purposes
+     */
+    std::vector<uint16_t> interpolate_forces_to_pwm(
+        const std::vector<double>& 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<std::vector<double>> coeffs contains the pair
+     * of coefficients
+     *
+     * @return std::uint16_t scalar pwm value
+     */
+    std::uint16_t force_to_pwm(double force,
+                               const std::vector<std::vector<double>>& 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<double>& 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<uint16_t>& 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<std::uint8_t, 2> i2c data
+     */
+    static constexpr std::array<std::uint8_t, 2> pwm_to_i2c_data(
+        std::uint16_t pwm) {
+        return {static_cast<std::uint8_t>((pwm >> 8) & 0xFF),
+                static_cast<std::uint8_t>(pwm & 0xFF)};
+    }
+};
+
+#endif  // THRUSTER_INTERFACE_ASV_DRIVER_HPP

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 <rclcpp/parameter_event_handler.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/int16_multi_array.hpp>
+#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<ThrusterParameters> thruster_parameters_;
+    std::vector<std::vector<double>> poly_coeffs_;
+
+    std::vector<double> thruster_forces_array_;
+    bool debug_flag_;
+
+    std::unique_ptr<ThrusterInterfaceASVDriver> thruster_driver_;
+    rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr
+        thruster_forces_subscriber_;
+    rclcpp::Publisher<std_msgs::msg::Int16MultiArray>::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<rclcpp::ParameterEventHandler> 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