From f12daff861abfa9c6d0c18558c922d1e0a03cbe6 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 12 Apr 2025 16:25:46 -0500 Subject: [PATCH] feat: fully remove socket_feedback class --- src/arm_pkg/arm_pkg/arm_node.py | 46 +++++++++++++------------ src/arm_pkg/arm_pkg/feedback_storage.py | 29 ---------------- 2 files changed, 24 insertions(+), 51 deletions(-) delete mode 100644 src/arm_pkg/arm_pkg/feedback_storage.py diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 531d372..f0d7201 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -25,7 +25,6 @@ from ament_index_python.packages import get_package_share_directory # from scipy.spatial.transform import Rotation as R from . import astra_arm -#from . import socket_feedback serial_pub = None thread = None @@ -33,7 +32,6 @@ thread = None - class SerialRelay(Node): def __init__(self): # Initialize node @@ -169,6 +167,10 @@ class SerialRelay(Node): angles[0] = 0.0 # Update the arm's current angles self.arm.update_angles(angles) + self.arm_feedback.axis0_angle = angles[0] + self.arm_feedback.axis1_angle = angles[1] + self.arm_feedback.axis2_angle = angles[2] + self.arm_feedback.axis3_angle = angles[3] self.get_logger().info(f"Angles: {angles}") else: self.get_logger().info("Invalid angle feedback input format") @@ -180,12 +182,10 @@ class SerialRelay(Node): # Extract the voltage from the string voltages_in = parts[3:7] # Convert the voltages to floats - v_bat = float(voltages_in[0]) / 100.0 - v_12 = float(voltages_in[1]) / 100.0 - v_5 = float(voltages_in[2]) / 100.0 - v_3 = float(voltages_in[3]) / 100.0 - # Update the arm's current voltages - self.arm_feedback.updateBusVoltages([v_bat, v_12, v_5, v_3]) + self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0 + self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0 + self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0 + self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0 else: self.get_logger().info("Invalid voltage feedback input format") @@ -197,10 +197,12 @@ class SerialRelay(Node): values_in = parts[3:7] # Convert the voltages to floats for i in range(4): - self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) - self. - self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) - self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc... + pass + + # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) else: self.get_logger().info("Invalid motor feedback input format") @@ -357,17 +359,17 @@ class SerialRelay(Node): def socket_pub_callback(self): # Create a SocketFeedback message and publish it - msg = SocketFeedback() - msg.bat_voltage = self.arm_feedback.bat_voltage - msg.voltage_12 = self.arm_feedback.voltage_12 - msg.voltage_5 = self.arm_feedback.voltage_5 - msg.voltage_3 = self.arm_feedback.voltage_3 - msg.joint_angles = self.arm_feedback.joint_angles - msg.joint_temps = self.arm_feedback.joint_temps - msg.joint_voltages = self.arm_feedback.joint_voltages - msg.joint_currents = self.arm_feedback.joint_currents + # msg = SocketFeedback() + # msg.bat_voltage = self.arm_feedback.bat_voltage + # msg.voltage_12 = self.arm_feedback.voltage_12 + # msg.voltage_5 = self.arm_feedback.voltage_5 + # msg.voltage_3 = self.arm_feedback.voltage_3 + # msg.joint_angles = self.arm_feedback.joint_angles + # msg.joint_temps = self.arm_feedback.joint_temps + # msg.joint_voltages = self.arm_feedback.joint_voltages + # msg.joint_currents = self.arm_feedback.joint_currents - self.socket_pub.publish(msg) #Publish feedback from arm + self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm self.arm.update_position() #Run FK and update the current position of the arm, using FK diff --git a/src/arm_pkg/arm_pkg/feedback_storage.py b/src/arm_pkg/arm_pkg/feedback_storage.py deleted file mode 100644 index 66c88d3..0000000 --- a/src/arm_pkg/arm_pkg/feedback_storage.py +++ /dev/null @@ -1,29 +0,0 @@ -# struct for socket feedback -class Feedback: - def __init__(self): - self.bat_voltage = 0.0 - self.voltage_12 = 0.0 - self.voltage_5 = 0.0 - self.voltage_3 = 0.0 - self.joint_angles = [0.0, 0.0, 0.0, 0.0] - self.joint_temps = [0.0, 0.0, 0.0, 0.0] - self.joint_voltages = [0.0, 0.0, 0.0, 0.0] - self.joint_currents = [0.0, 0.0, 0.0, 0.0] - - def updateBusVoltages(self, voltages): - self.bat_voltage = voltages[0] - self.voltage_12 = voltages[1] - self.voltage_5 = voltages[2] - self.voltage_3 = voltages[3] - - def updateJointVoltages(self, axis, voltage): - self.joint_voltages[axis] = voltage - - def updateJointCurrents(self, axis, current): - self.joint_currents[axis] = current - - def updateJointTemperatures(self, axis, temperature): - self.joint_temps[axis] = temperature - - def updateJointAngles(self, angles): - self.joint_angles = angles