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 2043938a..a306b75d 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 @@ -12,7 +12,6 @@ #include #include // Used for the close function -// Declare functions we can use namespace thruster_interface_asv_driver_lib { void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, int8_t *thrusterDirection, int16_t *offsetPWM, int16_t *minPWM, 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 29e8cd84..f1fd7f35 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 @@ -15,10 +15,9 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { std::vector<_ForcePWM> data; // Try opening the file and if suceeded fill our datastructure with all the - // values in the .CSV file + // values from the force mapping .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 @@ -29,6 +28,7 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { // 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 ForcePWMDataStructure; @@ -36,13 +36,16 @@ std::vector<_ForcePWM> _loadDataFromCSV(const std::string &filepath) { // 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 calculates forsces in grams + // 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); @@ -70,10 +73,12 @@ int16_t *_interpolate_force_to_pwm(float *forces) { // accordingly if (forces[i] <= _ForcePWMTable.front().force) { interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.front().pwm); // To small Force + static_cast(_ForcePWMTable.front().pwm); // Too small Force + } else if (forces[i] >= _ForcePWMTable.back().force) { interpolatedPWMArray[i] = - static_cast(_ForcePWMTable.back().pwm); // To big Force + static_cast(_ForcePWMTable.back().pwm); // Too big Force + } else { // Set temporary variables for interpolating // Initialize with the first element @@ -168,16 +173,9 @@ 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. _ForcePWMTable = _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]; @@ -190,10 +188,8 @@ void init(const std::string &pathToCSVFile, int8_t *thrusterMapping, // 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:] + // 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]; @@ -203,14 +199,14 @@ int16_t *drive_thrusters(float *thrusterForces) { // 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}; + float thrusterForcesChangedDirection[4] = {0.0, 0.0, 0.0, 0.0}; for (int8_t i = 0; i < 4; i++) { - thrusterForcesCahngedDirection[i] = + thrusterForcesChangedDirection[i] = thrusterForcesChangedMapping[i] * _thrusterDirection[i]; } // Interpolate forces to raw PWM values - int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesCahngedDirection); + int16_t *pwm = _interpolate_force_to_pwm(thrusterForcesChangedDirection); // Offset PWM for (int8_t i = 0; i < 4; i++) { 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 2c119835..a4ebf83c 100644 --- a/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp +++ b/motion/thruster_interface_asv/src/thruster_interface_asv_node.cpp @@ -4,12 +4,9 @@ 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 ---------- @@ -33,6 +30,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { // 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]; @@ -44,7 +42,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { // Builder for the object ---------- ThrusterInterfaceASVNode() : Node("thruster_interface_asv_node") { // Thruster Driver Setup ---------- - // Get filepath of .CSV file with ROS2 file path finder + // 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"; @@ -53,6 +51,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { 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]), @@ -63,6 +62,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { 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]), @@ -73,6 +73,7 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { 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]), @@ -82,13 +83,16 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { 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]), @@ -100,18 +104,18 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { offsetPWM, minPWM, maxPWM); // ROS Setup ---------- - // Initialize ROS2 subscribers role + // Initialize ROS2 thrusterForces subscriber _subscriberThrusterForces = this->create_subscription( "/thrust/thruster_forces", 5, std::bind(&ThrusterInterfaceASVNode::_thruster_forces_callback, this, std::placeholders::_1)); - // Initialize ROS2 publisher role + // Initialize ROS2 pwm publisher _publisherPWM = this->create_publisher("/pwm", 5); - // Initialize a never ending cycle that continuisly publishes and drives + // 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; @@ -126,13 +130,8 @@ class ThrusterInterfaceASVNode : public rclcpp::Node { }; int main(int argc, char *argv[]) { - // Start ROS2 rclcpp::init(argc, argv); - - // Start infinite loop of ROS2 Node rclcpp::spin(std::make_shared()); - - // Shut down ROS2 rclcpp::shutdown(); return 0; }