aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: fully remove socket_feedback class

Browse Source
This commit is contained in:
Tristan McGinnis
2025-04-12 16:25:46 -05:00
committed by David
parent e059f5cfec
commit f12daff861
2 changed files with 24 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -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

29
src/arm_pkg/arm_pkg/feedback_storage.py
View File

@@ -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