fixed embedded safety timeout on action -riley

This reverts commit 2dbdae8ade.

flake.lock

@@ -10,17 +10,17 @@
         ]
       },
       "locked": {
-        "lastModified": 1771573274,
+        "lastModified": 1771845042,
-        "narHash": "sha256-YlUVTWtCxXzobbcHTCrMMzwLSrzQwo5Yz57gYS8ie04=",
+        "narHash": "sha256-pGsb93ZlMhP3biy8S2eJc1sW+35vmaxlWHTbuwZDlQI=",
         "owner": "SHC-ASTRA",
         "repo": "astra_msgs",
-        "rev": "8acbd807907ca6bd045aab8e50e7af995fc9a211",
+        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
         "type": "github"
       },
       "original": {
         "owner": "SHC-ASTRA",
         "repo": "astra_msgs",
-        "rev": "8acbd807907ca6bd045aab8e50e7af995fc9a211",
+        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
         "type": "github"
       }
     },

flake.nix

@@ -5,7 +5,7 @@
     nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs";
     astra-msgs.url =
-      "github:SHC-ASTRA/astra_msgs/8acbd807907ca6bd045aab8e50e7af995fc9a211";
+      "github:SHC-ASTRA/astra_msgs/acabfd117d9711afc420612375b4e02f4ce4982d";
   };
 
   outputs =

src/bio_pkg/bio_pkg/bio_node.py

@@ -5,8 +5,8 @@ import time
 
 import rclpy
 from astra_msgs.action import BioVacuum
-from astra_msgs.msg import BioControl, BioDistributor, BioLaser, FaerieControl, ScytheControl, VicCAN
+from astra_msgs.msg import CitadelControl, FaerieControl, VicCAN
-from astra_msgs.srv import BioTestTube
+from astra_msgs.srv import BioTestTube, LibsSystem
 from rclpy.action import ActionServer
 from rclpy.node import Node
 from std_msgs.msg import Header, String
@@ -42,10 +42,11 @@ class SerialRelay(Node):
         )
         self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        self.distributor_sub = self.create_subscription(
+        # Messages
-            BioDistributor,
+        self.citadel_sub = self.create_subscription(
-            "/bio/control/distributor",
+            CitadelControl,
-            self.distributor_callback,
+            "/bio/control/citadel",
+            self.citadel_callback,
             10,
         )
 
@@ -56,24 +57,13 @@ class SerialRelay(Node):
             10,
         )
 
-        self.scythe_sub = self.create_subscription(
-            ScytheControl, 
-            "/bio/control/scythe", 
-            self.scythe_callback, 
-            10,
-        )
-
-        self.laser_sub = self.create_subscription(
-            ScytheControl, 
-            "/bio/control/laser", 
-            self.laser_callback, 
-            10,
-        )
-
         # Services
         self.test_tube_service = self.create_service(
             BioTestTube, "/bio/control/test_tube", self.test_tube_callback
         )
+        self.libs_service = self.create_service(
+            LibsSystem, "bio/control/libs_system", self.libs_callback
+        )
 
         # Actions
         self._action_server = ActionServer(
@@ -91,9 +81,6 @@ class SerialRelay(Node):
         except KeyboardInterrupt:
             pass
 
-    def send_control(self, msg: BioControl):
-        print("todo")
-
     def send_cmd(self, msg: str):
         # send to anchor node to relay
         output = String()
@@ -121,9 +108,9 @@ class SerialRelay(Node):
         self.get_logger().info(f"[Bio Anchor] {msg.data}")
         # no data planned to be received from citadel, not sure about lance
 
-    def distributor_callback(self, msg: BioDistributor):
+    def citadel_callback(self, msg: CitadelControl):
         distributor_arr = msg.distributor_id
-        # if (any in distributor > 
+        # Distributor Control
         vic_cmd = VicCAN(
             header=Header(stamp=self.get_clock().now().to_msg()),
             mcu_name="citadel",
@@ -136,17 +123,17 @@ class SerialRelay(Node):
             ],
         )
         self.anchor_tovic_pub_.publish(vic_cmd)
-    
+        # Move Scythe
-    def faerie_callback(self, msg: FaerieControl):
-        # msg.vibration_motor:
         vic_cmd = VicCAN(
             header=Header(stamp=self.get_clock().now().to_msg()),
             mcu_name="digit",
-            command_id=37,
+            command_id=42,
-            data=[float(msg.vibration_motor)]
+            data=[float(msg.move_scythe)],
         )
         self.anchor_tovic_pub_.publish(vic_cmd)
-        # msg.move_faerie:
+
+    def faerie_callback(self, msg: FaerieControl):
+        # Move Faerie
         vic_cmd = VicCAN(
             header=Header(stamp=self.get_clock().now().to_msg()),
             mcu_name="digit",
@@ -154,31 +141,22 @@ class SerialRelay(Node):
             data=[float(msg.move_faerie)],
         )
         self.anchor_tovic_pub_.publish(vic_cmd)
-        # msg.drill_speed
+        # Drill Speed
         vic_cmd = VicCAN(
             header=Header(stamp=self.get_clock().now().to_msg()),
             mcu_name="digit",
             command_id=19,
-            data=[float(msg.drill_speed)],
+            data=[
+                float(msg.drill_speed * 100)
+            ],  # change on embedded so we can go (-1,1)
         )
         self.anchor_tovic_pub_.publish(vic_cmd)
-
+        # Vanity Laser Control
-
-    def scythe_callback(self, msg: ScytheControl):
         vic_cmd = VicCAN(
             header=Header(stamp=self.get_clock().now().to_msg()),
             mcu_name="digit",
-            command_id=24,
+            command_id=28,
-            data=[float(msg.move_scythe)],
+            data=[float(msg.vanity_laser)],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
-
-    def laser_callback(self, msg: BioLaser):
-        vic_cmd = VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="digit",
-            command_id=24,
-            data=[float(msg.fire)],
         )
         self.anchor_tovic_pub_.publish(vic_cmd)
 
@@ -195,6 +173,9 @@ class SerialRelay(Node):
         self.anchor_tovic_pub_.publish(vic_cmd)
         return response
 
+    def libs_callback(self, request, response):
+        print("todo")
+
     def execute_vacuum(self, goal_handle):
         valve_id = int(goal_handle.request.valve_id)
         duty = int(goal_handle.request.fan_duty_cycle_percent)
@@ -210,20 +191,20 @@ class SerialRelay(Node):
             )
         )
 
-        # set fan duty cycle
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[float(duty)],
-            )
-        )
-
         feedback = BioVacuum.Feedback()
         start = time.time()
 
         while True:
+            # set fan duty cycle
+            self.anchor_tovic_pub_.publish(
+                VicCAN(
+                    header=Header(stamp=self.get_clock().now().to_msg()),
+                    mcu_name="citadel",
+                    command_id=19,
+                    data=[float(duty)],
+                )
+            )
+
             elapsed = int((time.time() - start) * 1000)
             remaining = max(0, total - elapsed)