worked on the reading part of the ros node

motion/thruster_interface/include/thruster_interface/thruster_interface.hpp

@@ -10,9 +10,19 @@
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <vector>
-
 using namespace std;
 
+#ifndef PWM_TABLE_HPP
+#define PWM_TABLE_HPP
+extern const int I2C_ADDRESS;
+extern const char *I2C_DEVICE;
+extern int8_t i2c_slave_addr;
+extern bool started;
+extern int8_t hardware_status;
+extern uint8_t software_killswitch;
+extern std::map<float, float> pwm_table;
+#endif
+
 //--------------------------INITIALISATION--------------------------
 void init(int &file);
 
@@ -27,5 +37,5 @@ uint8_t read_hardware_statusFromI2C(int file);
 
 //--------------------------INTERPOLATION--------------------------
 void get_pwm_table();
-uint16_t interpolate(float force);
-std::vector<uint16_t> interpolate_all(std::vector<float> &force_values);
+uint16_t interpolate(double force);
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values);

motion/thruster_interface/src/thruster_interface.cpp

@@ -3,58 +3,7 @@
 const int I2C_ADDRESS = 0x21;
 const char *I2C_DEVICE = "/dev/i2c-1";
 int8_t i2c_slave_addr = 0x21;
-
-bool started = false;
-
-int8_t hardware_status = 0;
-
-#ifndef PWM_TABLE_HPP
-#define PWM_TABLE_HPP
-
 std::map<float, float> pwm_table;
-std::vector<float> temperature;
-std::vector<uint16_t> pwm_values;
-
-#endif
-
-/**
-int main() {
-
-    std::vector<float> forces_values = {110000,5000,-7700,-1300000.15}; //change
-later has to come from somewhere uint8_t software_killswitch = 0; //0 if
-everything goes well and 1 if there is a problem ; change later has to come from
-somewhere
-
-    pwm_values.resize(4);
-    pwm_values = interpolate_all(forces_values);
-
-    int file;
-    if(started == false){
-        init(file);
-        started = true;
-    }
-
-// SENDING
-    send_status(software_killswitch, file);
-    send_pwm(pwm_values, file);
-
-
-// RECEIVING
-    temperature = readFloatsFromI2C(file);
-
-    for (size_t i = 0; i < temperature.size(); i++)
-    {
-        std::cout << i << "temperature value = " << temperature[i] << std::endl;
-    }
-
-    hardware_status = read_hardware_statusFromI2C(file);
-    cout << "hardware status = " << (uint16_t)(hardware_status) << endl;
-
-    close(file);
-
-    return 1;
-}
-**/
 
 //--------------------------FUNCTIONS--------------------------
 
@@ -178,7 +127,7 @@ void get_pwm_table() {
   file.close();
 }
 
-uint16_t interpolate(float force) {
+uint16_t interpolate(double force) {
 
   if (pwm_table.empty()) {
     get_pwm_table();
@@ -207,7 +156,7 @@ uint16_t interpolate(float force) {
   return pwm1 + (pwm2 - pwm1) * (force - force1) / (force2 - force1);
 }
 
-std::vector<uint16_t> interpolate_all(std::vector<float> &force_values) {
+std::vector<uint16_t> interpolate_all(std::vector<double> &force_values) {
   std::vector<uint16_t> interpolatedVector;
   // Interpolate each value in the input vector
   for (const auto &force : force_values) {

motion/thruster_interface/src/thruster_interface_node.cpp

@@ -1,22 +1,79 @@
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/float32.hpp"
 #include <thruster_interface/thruster_interface.hpp>
+//#include <vortex_msgs/msg/pwm.hpp>
+#include <vortex_msgs/msg/thruster_forces.hpp>
 
 using std::placeholders::_1;
 
+constexpr double newton_to_gram_conversion_factor = 101.97162;
+
+
+bool started = false;
+int8_t hardware_status = 0;
+uint8_t software_killswitch = 0;
+
 class MinimalSubscriber : public rclcpp::Node {
 public:
-  MinimalSubscriber() : Node("minimal_subscriber") {
-    subscription_ = this->create_subscription<std_msgs::msg::Float32>(
-        "float_topic", 10,
-        std::bind(&MinimalSubscriber::float_topic_callback, this, _1));
+  MinimalSubscriber() : Node("thruster_interface_node") {
+    subscription_ = this->create_subscription<vortex_msgs::msg::ThrusterForces>(
+        "thruster_topic", 10,
+        //std::bind(&MinimalSubscriber::float_topic_callback, this, _1));
+        std::bind(&MinimalSubscriber::thrusterForcesCallback, this, std::placeholders::_1));
+
   }
 
 private:
-  void float_topic_callback(const std_msgs::msg::Float32::SharedPtr msg) const {
-    RCLCPP_INFO(this->get_logger(), "I heard: '%u'", interpolate(msg->data));
+  void thrusterForcesCallback(const vortex_msgs::msg::ThrusterForces::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};
+
+     std::vector<uint16_t> pwm_values;
+     pwm_values.resize(4);
+     pwm_values = interpolate_all(forces_in_grams);
+
+     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]);
+      }
+
+//----------------------------------------------------------
+
+      int file;
+      if(started == false){
+          init(file);
+          started = true;
+      }
+
+  // SENDING
+      send_status(software_killswitch, file);
+      send_pwm(pwm_values, file);
+
+
+  // RECEIVING
+
+      std::vector<float> temperature;
+
+      temperature = readFloatsFromI2C(file);
+
+      for (size_t i = 0; i < temperature.size(); i++)
+      {
+          std::cout << i << "temperature value = " << temperature[i] << std::endl;
+      }
+
+      hardware_status = read_hardware_statusFromI2C(file);
+      cout << "hardware status = " << (uint16_t)(hardware_status) << endl;
+
+      close(file);
+
+
   }
-  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr subscription_;
+  rclcpp::Subscription<vortex_msgs::msg::ThrusterForces>::SharedPtr subscription_;
+
 };
 
 int main(int argc, char *argv[]) {