feat: update for new faerie arm

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -90,9 +90,9 @@ class SerialRelay(Node):
                 msg.data = output
                 if output.startswith("can_relay_fromvic,core"):
                     self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm"):
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
                     self.arm_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,bio"):
+                elif output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
                     self.bio_pub.publish(msg)
                 # msg = String()
                 # msg.data = output
@@ -135,16 +135,16 @@ def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
     if serial_pub:
         serial_pub.cleanup()
-      
+
 
 def main(args=None):
-   rclpy.init(args=args)
-   sys.excepthook = myexcepthook
+    rclpy.init(args=args)
+    sys.excepthook = myexcepthook
 
-   global serial_pub
+    global serial_pub
 
-   serial_pub = SerialRelay()
-   serial_pub.run()
+    serial_pub = SerialRelay()
+    serial_pub.run()
 
 if __name__ == '__main__':
     signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly

src/bio_pkg/bio_pkg/bio_node.py

@@ -9,6 +9,7 @@ import atexit
 import signal
 from std_msgs.msg import String
 from ros2_interfaces_pkg.msg import BioControl
+from ros2_interfaces_pkg.msg import BioFeedback
 
 serial_pub = None
 thread = None
@@ -25,10 +26,10 @@ class SerialRelay(Node):
 
         # Create publishers
         self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10)
-        #self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
+        self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10)
 
 
-        # Create subscribers\
+        # Create subscribers
         self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10)
 
         # Topics used in anchor mode
@@ -136,7 +137,7 @@ class SerialRelay(Node):
             self.send_cmd(command)        
         
 
-        # LSS
+        # LSS (SCYTHE)
         command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n"
         self.send_cmd(command)
         # Vibration Motor
@@ -150,18 +151,18 @@ class SerialRelay(Node):
         # To be reviewed before use#
 
         # Laser
-        command = "can_relay_tovic,faerie,28," + str(msg.laser) + "\n"
+        command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
         self.send_cmd(command)
-        
-        # # UV Light
-        # command = "can_relay_tovic,faerie,38," + str(msg.uvLight) + "\n"
-        # self.send_cmd(command)
 
-        # Drill
-        command = f"can_relay_tovic,faerie,19,{msg.drill_duty:.2f}\n"
+        # Drill (SCABBARD)
+        command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n"
         print(msg.drill_duty)
         self.send_cmd(command)
 
+        # Bio linear actuator
+        command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
+        self.send_cmd(command)
+
 
 
     def send_cmd(self, msg):