diff --git a/asv_setup/config/robots/freya.yaml b/asv_setup/config/robots/freya.yaml index d8b88c7d..5674ccbb 100644 --- a/asv_setup/config/robots/freya.yaml +++ b/asv_setup/config/robots/freya.yaml @@ -55,5 +55,5 @@ thruster_interface: publishing_rate: 1000 # [millisecond], Publishing 6 temperature values (ESC1, ESC2, ESC3, ESC4, AMBIENT1, AMBIENT2) # and the status received from the arduino controlling the ESC - PWM_min: 1300 # Recommended : 1100 - PWM_max: 1700 # Recommended : 1900 + PWM_min: 1100 # Recommended : 1100 + PWM_max: 1400 # Recommended : 1900 diff --git a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp index 2c32482a..26ccc329 100644 --- a/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp +++ b/motion/thruster_interface/include/thruster_interface/thruster_interface.hpp @@ -40,3 +40,19 @@ void get_pwm_table(); uint16_t interpolate(double force, int PWM_min, int PWM_max); std::vector interpolate_all(std::vector &force_values, int PWM_min = 1100, int PWM_max = 1900); + + +class I2C_Exception : public std::exception { +private: + string message = " "; +public: + I2C_Exception(string msg) { + message = msg; + } + + const char *what() const + + noexcept override{ + return message.c_str(); + } +}; \ No newline at end of file diff --git a/motion/thruster_interface/src/thruster_interface.cpp b/motion/thruster_interface/src/thruster_interface.cpp index 7d674e02..2224e8ac 100644 --- a/motion/thruster_interface/src/thruster_interface.cpp +++ b/motion/thruster_interface/src/thruster_interface.cpp @@ -11,9 +11,8 @@ std::map pwm_table; void init(int &file) { if ((file = open(I2C_DEVICE, O_RDWR)) < 0) { - std::cerr << "error, could not connect to i2c bus" << std::endl; close(file); - exit(EXIT_FAILURE); + throw I2C_Exception("error, could not connect to i2c bus"); } else { std::cout << "connected to i2c bus!!" << std::endl; } @@ -31,9 +30,8 @@ void init(int &file) { void send_status(int8_t status, int file) { if (write(file, &status, 1) != 1) { - std::cerr << "error, could not send status" << std::endl; close(file); - exit(EXIT_FAILURE); + throw I2C_Exception("error, could not send status"); } else { std::cout << "status data has been sent" << std::endl; } @@ -43,10 +41,8 @@ void send_pwm(std::vector pwm_values, int file) { std::vector bytes = pwm_to_bytes(pwm_values); if (static_cast(write(file, bytes.data(), bytes.size())) != bytes.size()) { - std::cerr << "error, could not send PWM data" << std::endl; + throw I2C_Exception("error, could not send PWM data"); close(file); - exit(EXIT_FAILURE); - } else { std::cout << "PWM data has been sent" << std::endl; } @@ -79,10 +75,9 @@ std::vector readFloatsFromI2C(int file) { // Read a block of bytes from the I2C device std::vector data(byte_length); if (read(file, data.data(), byte_length) != byte_length) { - std::cerr << "Failed to read data from the I2C device" << std::endl; + throw I2C_Exception("Failed to read data from the I2C device"); return floats; // Returns empty vector if reading fails } - // Convert the byte array to a list of floats memcpy(floats.data(), data.data(), byte_length); return floats; @@ -90,10 +85,8 @@ std::vector readFloatsFromI2C(int file) { uint8_t read_hardware_statusFromI2C(int file) { uint8_t hardware_status; - if (read(file, &hardware_status, 1) != 1) { - std::cerr << "Failed to read hardware status from the I2C device" - << std::endl; - exit(EXIT_FAILURE); + if (read(file, &hardware_status, 1) != 1) { + throw I2C_Exception("Failed to read hardware status from the I2C device"); } return hardware_status; } @@ -170,4 +163,5 @@ std::vector interpolate_all(std::vector &force_values, } return interpolatedVector; -} \ No newline at end of file +} + diff --git a/motion/thruster_interface/src/thruster_interface_node.cpp b/motion/thruster_interface/src/thruster_interface_node.cpp index b97d83ec..c7e9fe3b 100644 --- a/motion/thruster_interface/src/thruster_interface_node.cpp +++ b/motion/thruster_interface/src/thruster_interface_node.cpp @@ -3,7 +3,8 @@ #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/int8.hpp" #include -#include +//#include +#include "std_msgs/msg/float64_multi_array.hpp" using std::placeholders::_1; @@ -15,11 +16,16 @@ uint8_t software_killswitch; int8_t hardware_status = 0; class ThrusterInterface : public rclcpp::Node { +private: + +int PWM_min; +int PWM_max; + public: ThrusterInterface() : Node("thruster_interface_node") { subscription_thruster_forces = - this->create_subscription( + this->create_subscription( "thrust/thruster_forces", 10, std::bind(&ThrusterInterface::thrusterForcesCallback, this, std::placeholders::_1)); @@ -50,9 +56,9 @@ class ThrusterInterface : public rclcpp::Node { // get parameters from the freya.yaml file this->declare_parameter("thruster_interface.PWM_min", 500); - int PWM_min = this->get_parameter("thruster_interface.PWM_min").as_int(); + PWM_min = this->get_parameter("thruster_interface.PWM_min").as_int(); this->declare_parameter("thruster_interface.PWM_max", 500); - int PWM_max = this->get_parameter("thruster_interface.PWM_max").as_int(); + PWM_max = this->get_parameter("thruster_interface.PWM_max").as_int(); this->declare_parameter("thruster_interface.publishing_rate", 500); int publishing_rate = this->get_parameter("thruster_interface.publishing_rate").as_int(); @@ -70,29 +76,35 @@ class ThrusterInterface : public rclcpp::Node { //---------------------------------------------------------- void thrusterForcesCallback( - const vortex_msgs::msg::ThrusterForces::SharedPtr msg) const { + const std_msgs::msg::Float64MultiArray::SharedPtr msg) const { std::vector forces_in_grams = { - msg->thrust[0] * newton_to_gram_conversion_factor, - msg->thrust[1] * newton_to_gram_conversion_factor, - msg->thrust[2] * newton_to_gram_conversion_factor, - msg->thrust[3] * newton_to_gram_conversion_factor}; + msg->data[0] * newton_to_gram_conversion_factor, + msg->data[1] * newton_to_gram_conversion_factor, + msg->data[2] * newton_to_gram_conversion_factor, + msg->data[3] * newton_to_gram_conversion_factor}; std::vector pwm_values; pwm_values.resize(4); - pwm_values = interpolate_all(forces_in_grams); + pwm_values = interpolate_all(forces_in_grams,PWM_min,PWM_max); for (size_t i = 0; i < forces_in_grams.size(); ++i) { RCLCPP_INFO(this->get_logger(), "Force[%zu]: %f", i, forces_in_grams[i]); RCLCPP_INFO(this->get_logger(), "PWM[%zu]: %u", i, pwm_values[i]); } + try{ int file; init(file); // SENDING send_status(software_killswitch, file); send_pwm(pwm_values, file); + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + } } //---------------------------------------------------------- @@ -105,7 +117,8 @@ class ThrusterInterface : public rclcpp::Node { //---------------------------------------------------------- void publish_temperature_and_status() { - + try + { // RECEIVING int file; init(file); @@ -155,11 +168,17 @@ class ThrusterInterface : public rclcpp::Node { publisher_status->publish(message_status); close(file); + + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + } } //---------------------------------------------------------- - rclcpp::Subscription::SharedPtr + rclcpp::Subscription::SharedPtr subscription_thruster_forces; rclcpp::Subscription::SharedPtr @@ -183,3 +202,4 @@ int main(int argc, char *argv[]) { rclcpp::shutdown(); return 0; } +