Skip to content

Commit

Permalink
Added exception handling and changed message type from vortex msg to …
Browse files Browse the repository at this point in the history 
…std_msgs/msg/float64_multi_array
  • Loading branch information
“Vincent” committed Apr 8, 2024
1 parent 4afa343 commit 80e36f5
Show file tree
Hide file tree
Showing 4 changed files with 58 additions and 28 deletions.
4 changes: 2 additions & 2 deletions asv_setup/config/robots/freya.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
16 changes: 16 additions & 0 deletions motion/thruster_interface/include/thruster_interface/thruster_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,3 +40,19 @@ void get_pwm_table();
uint16_t interpolate(double force, int PWM_min, int PWM_max);
std::vector<uint16_t> interpolate_all(std::vector<double> &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();
}
};
22 changes: 8 additions & 14 deletions motion/thruster_interface/src/thruster_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,9 +11,8 @@ std::map<float, float> 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;
}
Expand All @@ -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;
}
Expand All @@ -43,10 +41,8 @@ void send_pwm(std::vector<uint16_t> pwm_values, int file) {
std::vector<uint8_t> bytes = pwm_to_bytes(pwm_values);
if (static_cast<size_t>(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;
}
Expand Down Expand Up @@ -79,21 +75,18 @@ std::vector<float> readFloatsFromI2C(int file) {
// Read a block of bytes from the I2C device
std::vector<uint8_t> 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;
}

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;
}
Expand Down Expand Up @@ -170,4 +163,5 @@ std::vector<uint16_t> interpolate_all(std::vector<double> &force_values,
}

return interpolatedVector;
}
}

44 changes: 32 additions & 12 deletions motion/thruster_interface/src/thruster_interface_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,8 @@
#include "std_msgs/msg/float32.hpp"
#include "std_msgs/msg/int8.hpp"
#include <thruster_interface/thruster_interface.hpp>
#include <vortex_msgs/msg/thruster_forces.hpp>
//#include <vortex_msgs/msg/thruster_forces.hpp>
#include "std_msgs/msg/float64_multi_array.hpp"

using std::placeholders::_1;

Expand All @@ -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<vortex_msgs::msg::ThrusterForces>(
this->create_subscription<std_msgs::msg::Float64MultiArray>(
"thrust/thruster_forces", 10,
std::bind(&ThrusterInterface::thrusterForcesCallback, this,
std::placeholders::_1));
Expand Down Expand Up @@ -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();
Expand All @@ -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<double> 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<uint16_t> 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';
}
}

//----------------------------------------------------------
Expand All @@ -105,7 +117,8 @@ class ThrusterInterface : public rclcpp::Node {
//----------------------------------------------------------

void publish_temperature_and_status() {

try
{
// RECEIVING
int file;
init(file);
Expand Down Expand Up @@ -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<vortex_msgs::msg::ThrusterForces>::SharedPtr
rclcpp::Subscription<std_msgs::msg::Float64MultiArray>::SharedPtr
subscription_thruster_forces;

rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr
Expand All @@ -183,3 +202,4 @@ int main(int argc, char *argv[]) {
rclcpp::shutdown();
return 0;
}

0 comments on commit 80e36f5

Please sign in to comment.