publishing works for full data float array

Author: JPGC04

pio_workspace/src/power_ros1_main.cpp

@@ -22,6 +22,7 @@ SETUP FOR ROS COMMUNICATION WITH THE POWER BOARD
 #include <ros.h>
 #include <auv_msgs/ThrusterMicroseconds.h>
 #include <std_msgs/Float32.h>
+#include <std_msgs/Float32MultiArray.h>
 
 // defines thruster pins
 #define BACK_L_PIN 2
@@ -35,8 +36,8 @@ SETUP FOR ROS COMMUNICATION WITH THE POWER BOARD
 
 #define LED_PIN 13
 
-#define ENABLE_VOLTAGE_SENSE true
-#define ENABLE_CURRENT_SENSE true
+#define ENABLE_VOLTAGE_SENSE false
+#define ENABLE_CURRENT_SENSE false
 
 // creates ADCSensors and TMP36 sensor objects
 ADCSensors adcSensors;
@@ -70,13 +71,18 @@ std_msgs::Float32 thruster8_current_msg;
 std_msgs::Float32 board_temperature_msg;
 std_msgs::Float32 teensy_temperature_msg;
 
+std_msgs::Float32MultiArray all_data_msg;
+float allData[21];
+
 // creates array of 8 thrusters
 Servo thrusters[8];
 
 // signals to push to thrusters
 uint16_t microseconds[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
 const uint16_t offCommand[] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};
 
+float thrusterForce = -1.0;
+
 float Bvoltages[2]; // array for 2 battery voltage sensing
 float Tcurrents[8]; // array for 8 thrusters current sensing
 float boardTemperature;
@@ -99,6 +105,10 @@ void commandCb(const auv_msgs::ThrusterMicroseconds &tc) {
     memcpy(microseconds, tc.microseconds, 8 * sizeof(uint16_t));
 }
 
+void thruster_force_Cb(const std_msgs::Float32 &tc) {
+    thrusterForce = tc.data;
+}
+
 // attaches and arms thrusters
 void initThrusters() {
     thrusters[BACK_L].attach(BACK_L_PIN);
@@ -129,6 +139,9 @@ ros::Publisher thruster8_current("/power/thrusters/current/8", &thruster8_curren
 ros::Publisher board_temperature("/power/board/temperature", &board_temperature_msg);
 ros::Publisher teensy_temperature("/power/teensy/temperature", &teensy_temperature_msg);
 
+ros::Subscriber<std_msgs::Float32> thrusterforcesub("/thruster/force", &thruster_force_Cb);
+ros::Publisher alldatapub("/all/data", &all_data_msg);
+
 // senses the battery voltages, thruster currents, and board and teensy temperatures
 void senseData() {
     float* voltagePtr = adcSensors.senseVoltage();
@@ -182,6 +195,39 @@ void publishData() {
     teensy_temperature.publish(&teensy_temperature_msg);
 }
 
+void allDataFunction() {
+    allData[0] = microseconds[0];
+    allData[1] = microseconds[1];
+    allData[2] = microseconds[2];
+    allData[3] = microseconds[3];
+    allData[4] = microseconds[4];
+    allData[5] = microseconds[5];
+    allData[6] = microseconds[6];
+    allData[7] = microseconds[7];
+
+    allData[8] = thrusterForce;
+
+    allData[9] = Tcurrents[0];
+    allData[10] = Tcurrents[1];
+    allData[11] = Tcurrents[2];
+    allData[12] = Tcurrents[3];
+    allData[13] = Tcurrents[4];
+    allData[14] = Tcurrents[5];
+    allData[15] = Tcurrents[6];
+    allData[16] = Tcurrents[7];
+
+    allData[17] = Bvoltages[0];
+    allData[18] = Bvoltages[1];
+
+    allData[19] = boardTemperature;
+
+    allData[20] = teensyTemperature;
+
+    all_data_msg.data = allData;
+
+    alldatapub.publish(&all_data_msg);
+}
+
 // setup all thrusters and sensors and setup node handler for subscribing and advertising
 void power_ros1_setup() {
     pinMode(LED_PIN, OUTPUT);
@@ -206,13 +252,20 @@ void power_ros1_setup() {
     nh.advertise(thruster8_current);
     nh.advertise(board_temperature);
     nh.advertise(teensy_temperature);
+    nh.subscribe(thrusterforcesub);
+
+    all_data_msg.data_length = 21;
+
+    nh.advertise(alldatapub);
 }
 
 void power_ros1_loop() {
     updateThrusters(microseconds); 
 
     publishData();
 
+    allDataFunction();
+
     nh.spinOnce();
 
     delay(10);

pio_workspace/src/thruster_tests_main.cpp

@@ -35,7 +35,7 @@ void sensor_setup() {
     boolean _tare = true;
     LoadCell.start(stabilizingtime, _tare);
     if (LoadCell.getTareTimeoutFlag()) {
-        while (1);
+        //while (1);
     } else {
         LoadCell.setCalFactor(calibrationValue);
     }

pio_workspace/test/thruster_tests/commands.txt

@@ -12,6 +12,7 @@ DOCKER:
 sudo apt update
 sudo apt install tmux -y
 sudo apt install git -y
+sudo apt install vim -y
 sudo apt install -y ros-noetic-rosserial
 sudo apt install python3-catkin-tools -y
 sudo apt install ros-noetic-rosserial-arduino ros-noetic-rosserial -y