diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 36f803a..531d372 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -25,41 +25,12 @@ 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 -# struct for socket feedback -class SocketFeedback: - 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 - @@ -68,9 +39,6 @@ class SerialRelay(Node): # Initialize node super().__init__("arm_node") - #output to console - self.get_logger().info("Starting arm node...") - # Get launch mode parameter self.declare_parameter('launch_mode', 'arm') self.launch_mode = self.get_parameter('launch_mode').value @@ -230,6 +198,7 @@ class SerialRelay(Node): # 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) else: diff --git a/src/arm_pkg/arm_pkg/astra_arm.py b/src/arm_pkg/arm_pkg/astra_arm.py index 80bbe61..df89790 100644 --- a/src/arm_pkg/arm_pkg/astra_arm.py +++ b/src/arm_pkg/arm_pkg/astra_arm.py @@ -21,7 +21,7 @@ class Arm: def __init__(self, urdf_name): self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid # URDF file path - self.urdf = os.path.join(get_package_share_directory('ik_pkg'), urdf_name) + self.urdf = os.path.join(get_package_share_directory('arm_pkg'), urdf_name) # IKpy Chain self.chain = Chain.from_urdf_file(self.urdf) diff --git a/src/arm_pkg/arm_pkg/feedback_storage.py b/src/arm_pkg/arm_pkg/feedback_storage.py new file mode 100644 index 0000000..66c88d3 --- /dev/null +++ b/src/arm_pkg/arm_pkg/feedback_storage.py @@ -0,0 +1,29 @@ +# 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