feat: actually add digit feedback

src/arm_pkg/arm_pkg/arm_node.py

@@ -10,7 +10,8 @@ import signal
 from std_msgs.msg import String
 from ros2_interfaces_pkg.msg import ArmManual
 from ros2_interfaces_pkg.msg import ArmIK
-from ros2_interfaces_pkg.msg import ArmFeedback
+from ros2_interfaces_pkg.msg import SocketFeedback
+from ros2_interfaces_pkg.msg import DigitFeedback
 
 serial_pub = None
 thread = None
@@ -27,7 +28,8 @@ class SerialRelay(Node):
 
         # Create publishers
         self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
-        self.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10)
+        self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
+        self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10)
         self.feedback_timer = self.create_timer(1.0, self.publish_feedback)
 
         # Create subscribers
@@ -39,7 +41,8 @@ class SerialRelay(Node):
             self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
             self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
 
-        self.arm_feedback = ArmFeedback()
+        self.arm_feedback = SocketFeedback()
+        self.digit_feedback = DigitFeedback()
 
         # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
         if self.launch_mode == 'arm':
@@ -188,15 +191,19 @@ class SerialRelay(Node):
                 # Extract the voltage from the string
                 voltages_in = parts[3:7]
                 # Convert the voltages to floats
-                self.arm_feedback.digit_bat_voltage = float(voltages_in[0]) / 100.0
-                self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0
-                self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0
-                self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0
+                self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0
+                self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0
+                self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0
+        elif output.startswith("can_relay_fromvic,digit,55"):
+            parts = msg.split(",")
+            if len(parts) >= 4:
+                self.digit_feedback.wrist_angle = float(parts[3])
         else:
             return
 
     def publish_feedback(self):
         self.socket_pub.publish(self.arm_feedback)
+        self.digit_pub.publish(self.digit_feedback)
 
     def updateAngleFeedback(self, msg):
                 # Angle feedbacks,

src/bio_pkg/bio_pkg/bio_node.py

@@ -184,7 +184,7 @@ class SerialRelay(Node):
         parts = str(output.strip()).split(",")
         self.get_logger().info(f"[Bio Anchor] {msg.data}")
 
-        if output.startswith("can_relay_fromvic,citadel,54"):#bat, 12, 5, 3, Voltage readings * 100
+        if output.startswith("can_relay_fromvic,citadel,54"):  # bat, 12, 5, Voltage readings * 100
             self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
             self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
             self.bio_feedback.voltage_5 = float(parts[5]) / 100.0