fixed embedded safety timeout on action -riley

This reverts commit 2dbdae8ade.

.gitmodules

@@ -1,6 +1,3 @@
 [submodule "src/astra_description"]
 	path = src/astra_descriptions
 	url = ../astra_descriptions
-[submodule "src/astra_msgs"]
-	path = src/astra_msgs
-	url = git@github.com:SHC-ASTRA/astra_msgs.git

auto_start/auto_start_anchor.sh

@@ -14,12 +14,8 @@ echo "[INFO] Network interface is up!"
 # Your actual ROS node start command goes here
 echo "[INFO] Starting ROS node..."
 
-# Source ROS 2 Humble setup script
+# Source ROS 2 Humble setup script (if we aren't using nix)
-if command -v nixos-rebuild; then
+command -v ros2 || source /opt/ros/humble/setup.bash
-    echo "[INFO] running on NixOS"
-else
-    source /opt/ros/humble/setup.bash
-fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/auto_start_headless_full.sh

@@ -15,11 +15,7 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-if command -v nixos-rebuild; then
+command -v ros2 || source /opt/ros/humble/setup.bash
-    echo "[INFO] running on NixOS"
-else
-    source /opt/ros/humble/setup.bash
-fi
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/start_rosbag.sh

@@ -17,11 +17,7 @@ done
 echo "[INFO] Network interface is up!"
 
 
-if command -v nixos-rebuild; then
+command -v ros2 || source /opt/ros/humble/setup.bash
-    echo "[INFO] running on NixOS"
-else
-    source /opt/ros/humble/setup.bash
-fi
 source $ANCHOR_WS/install/setup.bash
 [[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
 

flake.lock

@@ -1,5 +1,29 @@
 {
   "nodes": {
+    "astra-msgs": {
+      "inputs": {
+        "nix-ros-overlay": "nix-ros-overlay",
+        "nixpkgs": [
+          "astra-msgs",
+          "nix-ros-overlay",
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1771845042,
+        "narHash": "sha256-pGsb93ZlMhP3biy8S2eJc1sW+35vmaxlWHTbuwZDlQI=",
+        "owner": "SHC-ASTRA",
+        "repo": "astra_msgs",
+        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
+        "type": "github"
+      },
+      "original": {
+        "owner": "SHC-ASTRA",
+        "repo": "astra_msgs",
+        "rev": "acabfd117d9711afc420612375b4e02f4ce4982d",
+        "type": "github"
+      }
+    },
     "flake-utils": {
       "inputs": {
         "systems": "systems"
@@ -18,17 +42,55 @@
         "type": "github"
       }
     },
+    "flake-utils_2": {
+      "inputs": {
+        "systems": "systems_2"
+      },
+      "locked": {
+        "lastModified": 1731533236,
+        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "flake-utils",
+        "type": "github"
+      }
+    },
     "nix-ros-overlay": {
       "inputs": {
         "flake-utils": "flake-utils",
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770108954,
+        "lastModified": 1770622967,
-        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
+        "narHash": "sha256-1LYjTugPSCa/5NkP6/dcZLH5TQYj3R8mAZ/9dgd7jDM=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
+        "rev": "d1b9f17eba909116356436d46b5192d299c6b49a",
+        "type": "github"
+      },
+      "original": {
+        "owner": "lopsided98",
+        "ref": "develop",
+        "repo": "nix-ros-overlay",
+        "type": "github"
+      }
+    },
+    "nix-ros-overlay_2": {
+      "inputs": {
+        "flake-utils": "flake-utils_2",
+        "nixpkgs": "nixpkgs_2"
+      },
+      "locked": {
+        "lastModified": 1771404745,
+        "narHash": "sha256-UVP3TsQJ4PezyQG3B1SsgsTxz32XBVzplJ/cgq7v/uk=",
+        "owner": "lopsided98",
+        "repo": "nix-ros-overlay",
+        "rev": "7cdf7b44ff186869baedbb3b82e5b409bb3e8dd9",
         "type": "github"
       },
       "original": {
@@ -54,9 +116,26 @@
         "type": "github"
       }
     },
+    "nixpkgs_2": {
+      "locked": {
+        "lastModified": 1759381078,
+        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
+        "owner": "NixOS",
+        "repo": "nixpkgs",
+        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
+        "type": "github"
+      },
+      "original": {
+        "owner": "lopsided98",
+        "ref": "nix-ros",
+        "repo": "nixpkgs",
+        "type": "github"
+      }
+    },
     "root": {
       "inputs": {
-        "nix-ros-overlay": "nix-ros-overlay",
+        "astra-msgs": "astra-msgs",
+        "nix-ros-overlay": "nix-ros-overlay_2",
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
@@ -77,6 +156,21 @@
         "repo": "default",
         "type": "github"
       }
+    },
+    "systems_2": {
+      "locked": {
+        "lastModified": 1681028828,
+        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
+        "owner": "nix-systems",
+        "repo": "default",
+        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
+        "type": "github"
+      },
+      "original": {
+        "owner": "nix-systems",
+        "repo": "default",
+        "type": "github"
+      }
     }
   },
   "root": "root",

flake.nix

@@ -3,15 +3,13 @@
 
   inputs = {
     nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs";
+    astra-msgs.url =
+      "github:SHC-ASTRA/astra_msgs/acabfd117d9711afc420612375b4e02f4ce4982d";
   };
 
   outputs =
-    {
+    { self, nix-ros-overlay, nixpkgs, astra-msgs }:
-      self,
-      nix-ros-overlay,
-      nixpkgs,
-    }:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
       let
@@ -19,26 +17,31 @@
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
         };
+
+        astra_msgs_pkgs = astra-msgs.packages.${system};
+        rosDistro = "humble";
       in
       {
         devShells.default = pkgs.mkShell {
           name = "ASTRA Anchor";
+
           packages = with pkgs; [
             colcon
-            (python313.withPackages (
+            astra_msgs_pkgs.astra-msgs
-              p: with p; [
+
+            (pkgs.rosPackages.${rosDistro}.python3.withPackages (p: with p; [
               pyserial
               pygame
               scipy
               crccheck
               black
-              ]
+            ]))
-            ))
+
-            (
+            (with rosPackages.${rosDistro};
-              with rosPackages.humble;
               buildEnv {
                 paths = [
                   ros-core
+                  rqt-graph
                   ros2cli
                   ros2run
                   ros2bag
@@ -74,11 +77,14 @@
                   ros2-controllers
                   chomp-motion-planner
                 ];
-              }
+              })
-            )
           ];
+
+          env = {
+            ASTRAMSGS = "${astra-msgs.outPath}";
+          };
+
           shellHook = ''
-            # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
           '';
@@ -88,6 +94,7 @@
 
   nixConfig = {
     extra-substituters = [ "https://ros.cachix.org" ];
-    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
+    extra-trusted-public-keys =
+      [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
   };
 }

src/anchor_pkg/launch/rover.launch.py

@@ -1,21 +1,13 @@
 #!/usr/bin/env python3
 
 from launch import LaunchDescription
-from launch.actions import (
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-    DeclareLaunchArgument,
-    OpaqueFunction,
-    Shutdown,
-    IncludeLaunchDescription,
-)
 from launch.substitutions import (
     LaunchConfiguration,
     ThisLaunchFileDir,
     PathJoinSubstitution,
 )
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.conditions import IfCondition
 from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
 # Prevent making __pycache__ directories
@@ -48,10 +40,7 @@ def launch_setup(context, *args, **kwargs):
                 executable="core",  # change as needed
                 name="core",
                 output="both",
-                parameters=[
+                parameters=[{"launch_mode": mode}],
-                    {"launch_mode": mode},
-                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
-                ],
                 on_exit=Shutdown(),
             )
         )
@@ -144,46 +133,4 @@ def generate_launch_description():
         description="Launch mode: arm, core, bio, anchor, or ptz",
     )
 
-    ros2_control_arg = DeclareLaunchArgument(
+    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
-        "use_ros2_control",
-        default_value="false",
-        description="Whether to use DiffDriveController for driving instead of direct Twist",
-    )
-
-    rsp = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("core_description"),
-                    "launch",
-                    "robot_state_publisher.launch.py",
-                ]
-            )
-        ),
-        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
-        launch_arguments={("hardware_mode", "physical")},
-    )
-
-    controllers = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            PathJoinSubstitution(
-                [
-                    FindPackageShare("core_description"),
-                    "launch",
-                    "spawn_controllers.launch.py",
-                ]
-            )
-        ),
-        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
-        launch_arguments={("hardware_mode", "physical")},
-    )
-
-    return LaunchDescription(
-        [
-            declare_arg,
-            ros2_control_arg,
-            rsp,
-            controllers,
-            OpaqueFunction(function=launch_setup),
-        ]
-    )

src/bio_pkg/bio_pkg/bio_node.py

@@ -1,241 +1,251 @@
-import rclpy
+import signal
-from rclpy.node import Node
-import serial
 import sys
 import threading
-import os
-import glob
 import time
-import atexit
+
-import signal
+import rclpy
-from std_msgs.msg import String
+from astra_msgs.action import BioVacuum
-from astra_msgs.msg import BioControl
+from astra_msgs.msg import CitadelControl, FaerieControl, VicCAN
-from astra_msgs.msg import BioFeedback
+from astra_msgs.srv import BioTestTube, LibsSystem
+from rclpy.action import ActionServer
+from rclpy.node import Node
+from std_msgs.msg import Header, String
 
 serial_pub = None
 thread = None
 
+# used to verify the length of an incoming VicCAN feedback message
+# key is VicCAN command_id, value is expected length of data list
+viccan_citadel_msg_len_dict = {
+    # empty because not expecting any VicCAN from citadel atm
+}
+viccan_lance_msg_len_dict = {
+    # empty because not sure what VicCAN commands LANCE sends
+}
+
 
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("bio_node")
 
-        # Get launch mode parameter
+        # Anchor Topics
-        self.declare_parameter("launch_mode", "bio")
+        self.anchor_fromvic_sub_ = self.create_subscription(
-        self.launch_mode = self.get_parameter("launch_mode").value
+            VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20
-        self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
+        )
-
+        self.anchor_tovic_pub_ = self.create_publisher(
-        # Create publishers
+            VicCAN, "/anchor/to_vic/relay", 20
-        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
-        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
-
-        # Create subscribers
-        self.control_sub = self.create_subscription(
-            BioControl, "/bio/control", self.send_control, 10
         )
 
-        # Create a publisher for telemetry
-        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
-
-        # Topics used in anchor mode
-        if self.launch_mode == "anchor":
         self.anchor_sub = self.create_subscription(
             String, "/anchor/bio/feedback", self.anchor_feedback, 10
         )
         self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        self.bio_feedback = BioFeedback()
+        # Messages
-
+        self.citadel_sub = self.create_subscription(
-        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
+            CitadelControl,
-        if self.launch_mode == "bio":
+            "/bio/control/citadel",
-            # Loop through all serial devices on the computer to check for the MCU
+            self.citadel_callback,
-            self.port = None
+            10,
-            for i in range(2):
-                try:
-                    # connect and send a ping command
-                    set_port = (
-                        "/dev/ttyACM0"  # MCU is controlled through GPIO pins on the PI
         )
-                    ser = serial.Serial(set_port, 115200, timeout=1)
-                    # print(f"Checking port {port}...")
-                    ser.write(b"ping\n")
-                    response = ser.read_until("\n")
 
-                    # if pong is in response, then we are talking with the MCU
+        self.faerie_sub = self.create_subscription(
-                    if b"pong" in response:
+            FaerieControl,
-                        self.port = set_port
+            "/bio/control/faerie",
-                        self.get_logger().info(f"Found MCU at {set_port}!")
+            self.faerie_callback,
-                        break
+            10,
-                except:
-                    pass
-
-            if self.port is None:
-                self.get_logger().info(
-                    "Unable to find MCU... please make sure it is connected."
         )
-                time.sleep(1)
-                sys.exit(1)
 
-            self.ser = serial.Serial(self.port, 115200)
+        # Services
-            atexit.register(self.cleanup)
+        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(
+            self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
+        )
 
     def run(self):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
 
-        # if in arm mode, will need to read from the MCU
-
         try:
             while rclpy.ok():
-                if self.launch_mode == "bio":
-                    if self.ser.in_waiting:
-                        self.read_mcu()
-                    else:
                 time.sleep(0.1)
         except KeyboardInterrupt:
             pass
-        finally:
-            self.cleanup()
-
-    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
-    def read_mcu(self):
-        try:
-            output = str(self.ser.readline(), "utf8")
-            if output:
-                self.get_logger().info(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-        except serial.SerialException:
-            self.get_logger().info("SerialException caught... closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
-        except TypeError as e:
-            self.get_logger().info(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
-        except Exception as e:
-            print(f"Exception: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
-
-    def send_ik(self, msg):
-        pass
-
-    def send_control(self, msg: BioControl):
-        # CITADEL Control Commands
-        ################
-
-        # Chem Pumps, only send if not zero
-        if msg.pump_id != 0:
-            command = (
-                "can_relay_tovic,citadel,27,"
-                + str(msg.pump_id)
-                + ","
-                + str(msg.pump_amount)
-                + "\n"
-            )
-            self.send_cmd(command)
-        # Fans, only send if not zero
-        if msg.fan_id != 0:
-            command = (
-                "can_relay_tovic,citadel,40,"
-                + str(msg.fan_id)
-                + ","
-                + str(msg.fan_duration)
-                + "\n"
-            )
-            self.send_cmd(command)
-        # Servos, only send if not zero
-        if msg.servo_id != 0:
-            command = (
-                "can_relay_tovic,citadel,25,"
-                + str(msg.servo_id)
-                + ","
-                + str(int(msg.servo_state))
-                + "\n"
-            )
-            self.send_cmd(command)
-
-        # LSS (SCYTHE)
-        command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
-        # self.send_cmd(command)
-
-        # Vibration Motor
-        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
-        # self.send_cmd(command)
-
-        # FAERIE Control Commands
-        ################
-
-        # To be reviewed before use#
-
-        # Laser
-        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-        # self.send_cmd(command)
-
-        # Drill (SCABBARD)
-        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
-        # 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: str):
-        if (
+        # send to anchor node to relay
-            self.launch_mode == "anchor"
-        ):  # if in anchor mode, send to anchor node to relay
         output = String()
         output.data = msg
         self.anchor_pub.publish(output)
-        elif self.launch_mode == "bio":  # if in standalone mode, send to MCU directly
+
-            self.get_logger().info(f"[Bio to MCU] {msg}")
+    def relay_fromvic(self, msg: VicCAN):
-            self.ser.write(bytes(msg, "utf8"))
+        # self.get_logger().info(msg)
+        if msg.mcu_name == "citadel":
+            self.process_fromvic_citadel(msg)
+
+    def process_fromvic_citadel(self, msg: VicCAN):
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_citadel_msg_len_dict:
+            expected_len = viccan_citadel_msg_len_dict[msg.command_id]
+            if len(msg.data) != expected_len:
+                self.get_logger().warning(
+                    f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
+                )
+            return
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
-        # self.get_logger().info(f"[Bio Anchor] {msg.data}")
+        self.get_logger().info(f"[Bio Anchor] {msg.data}")
+        # no data planned to be received from citadel, not sure about lance
 
-        if output.startswith(
+    def citadel_callback(self, msg: CitadelControl):
-            "can_relay_fromvic,citadel,54"
+        distributor_arr = msg.distributor_id
-        ):  # bat, 12, 5, Voltage readings * 100
+        # Distributor Control
-            self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
+        vic_cmd = VicCAN(
-            self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
+            header=Header(stamp=self.get_clock().now().to_msg()),
-            self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
+            mcu_name="citadel",
-        elif output.startswith("can_relay_fromvic,digit,57"):
+            command_id=40,
-            self.bio_feedback.drill_temp = float(parts[3])
+            data=[
-            self.bio_feedback.drill_humidity = float(parts[4])
+                clamp_short(distributor_arr[0]),
+                clamp_short(distributor_arr[1]),
+                clamp_short(distributor_arr[2]),
+                0,
+            ],
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
+        # Move Scythe
+        vic_cmd = VicCAN(
+            header=Header(stamp=self.get_clock().now().to_msg()),
+            mcu_name="digit",
+            command_id=42,
+            data=[float(msg.move_scythe)],
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
 
-    def publish_feedback(self):
+    def faerie_callback(self, msg: FaerieControl):
-        self.feedback_pub.publish(self.bio_feedback)
+        # Move Faerie
+        vic_cmd = VicCAN(
+            header=Header(stamp=self.get_clock().now().to_msg()),
+            mcu_name="digit",
+            command_id=42,
+            data=[float(msg.move_faerie)],
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
+        # 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 * 100)
+            ],  # change on embedded so we can go (-1,1)
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
+        # Vanity Laser Control
+        vic_cmd = VicCAN(
+            header=Header(stamp=self.get_clock().now().to_msg()),
+            mcu_name="digit",
+            command_id=28,
+            data=[float(msg.vanity_laser)],
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
 
-    @staticmethod
+    def test_tube_callback(self, request, response):
-    def list_serial_ports():
+        vic_cmd = VicCAN(
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
+            header=Header(stamp=self.get_clock().now().to_msg()),
-        # return glob.glob("/dev/tty[A-Za-z]*")
+            mcu_name="citadel",
+            command_id=40,
+            data=[
+                float(int(request.tube_id)),
+                float(request.milliliters),
+            ],
+        )
+        self.anchor_tovic_pub_.publish(vic_cmd)
+        return response
 
-    def cleanup(self):
+    def libs_callback(self, request, response):
-        print("Cleaning up...")
+        print("todo")
-        try:
+
-            if self.ser.is_open:
+    def execute_vacuum(self, goal_handle):
-                self.ser.close()
+        valve_id = int(goal_handle.request.valve_id)
-        except Exception as e:
+        duty = int(goal_handle.request.fan_duty_cycle_percent)
-            exit(0)
+        total = goal_handle.request.fan_time_ms
+
+        # open valve
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                header=Header(stamp=self.get_clock().now().to_msg()),
+                mcu_name="citadel",
+                command_id=40,
+                data=[float(valve_id)],
+            )
+        )
+
+        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)
+
+            feedback.fan_time_remaining_ms = remaining
+            goal_handle.publish_feedback(feedback)
+
+            if remaining == 0:
+                break
+
+            time.sleep(0.1)
+
+        # stop fan
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                header=Header(stamp=self.get_clock().now().to_msg()),
+                mcu_name="citadel",
+                command_id=19,
+                data=[0.0],
+            )
+        )
+
+        # close valve
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                header=Header(stamp=self.get_clock().now().to_msg()),
+                mcu_name="citadel",
+                command_id=40,
+                data=[-1.0],
+            )
+        )
+
+        goal_handle.succeed()
+        return BioVacuum.Result()
+
+
+def clamp_short(x: int) -> int:
+    return max(-32768, min(32767, x))
 
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
-    if serial_pub:
-        serial_pub.cleanup()
 
 
 def main(args=None):

src/core_pkg/core_pkg/core_node.py

@@ -66,10 +66,6 @@ class SerialRelay(Node):
         self.launch_mode = self.get_parameter("launch_mode").value
         self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
 
-        self.declare_parameter("use_ros2_control", False)
-        self.use_ros2_control = self.get_parameter("use_ros2_control").value
-        self.get_logger().info(f"Use ros2_control: {self.use_ros2_control}")
-
         ##################################################
         # Topics
 
@@ -89,12 +85,6 @@ class SerialRelay(Node):
 
         # Control
 
-        if self.use_ros2_control:
-            # Joint state control for topic-based controller
-            self.joint_command_sub_ = self.create_subscription(
-                JointState, "/core/joint_commands", self.joint_command_callback, 2
-            )
-        else:
         # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
         self.cmd_vel_sub_ = self.create_subscription(
             TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
@@ -110,7 +100,9 @@ class SerialRelay(Node):
             self.control_state_callback,
             qos_profile=control_qos,
         )
-            self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        self.twist_max_duty = (
+            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        )
 
         # Feedback
 
@@ -130,7 +122,7 @@ class SerialRelay(Node):
         )  # TODO: not sure about this
         # Joint states for topic-based controller
         self.joint_state_pub_ = self.create_publisher(
-            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
         )
         # IMU (embedded BNO-055)
         self.imu_pub_ = self.create_publisher(
@@ -156,7 +148,6 @@ class SerialRelay(Node):
 
         # Old
 
-        if not self.use_ros2_control:
         # /core/control
         self.control_sub = self.create_subscription(
             CoreControl, "/core/control", self.send_controls, 10
@@ -292,40 +283,6 @@ class SerialRelay(Node):
 
         # print(f"[Sys] Relaying: {command}")
 
-    def joint_command_callback(self, msg: JointState):
-        # So... topic based control node publishes JointState messages over /joint_commands
-        #  with len(msg.name) == 5 and len(msg.velocity) == 4... all 5 non-fixed joints
-        #  are included in msg.name, but ig it is implied that msg.velocity only
-        #  includes velocities for the commanded joints (ros__parameters.joints).
-        # So, this will be much more hacky and less adaptable than I would like it to be.
-        if len(msg.name) != 5 or len(msg.velocity) != 4 or len(msg.position) != 0:
-            self.get_logger().warning(
-                f"Received joint control message with unexpected number of joints. Ignoring."
-            )
-            return
-        if msg.name != [
-            "left_suspension_joint",
-            "bl_wheel_joint",
-            "br_wheel_joint",
-            "fl_wheel_joint",
-            "fr_wheel_joint",
-        ]:
-            self.get_logger().warning(
-                f"Received joint control message with unexpected name[]. Ignoring."
-            )
-            return
-
-        (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
-
-        bl_rpm = radps_to_rpm(bl_vel) * CORE_GEAR_RATIO
-        br_rpm = radps_to_rpm(br_vel) * CORE_GEAR_RATIO
-        fl_rpm = radps_to_rpm(fl_vel) * CORE_GEAR_RATIO
-        fr_rpm = radps_to_rpm(fr_vel) * CORE_GEAR_RATIO
-
-        self.send_viccan(
-            20, [fl_rpm, bl_rpm, fr_rpm, br_rpm]
-        )  # order expected by embedded
-
     def cmd_vel_callback(self, msg: TwistStamped):
         linear = msg.twist.linear.x
         angular = -msg.twist.angular.z
@@ -571,27 +528,22 @@ class SerialRelay(Node):
                 match motorId:
                     case 1:
                         motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_wheel_joint"]
+                        joint_state_msg.name = ["fl_motor_joint"]
                     case 2:
                         motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_wheel_joint"]
+                        joint_state_msg.name = ["bl_motor_joint"]
                     case 3:
                         motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_wheel_joint"]
+                        joint_state_msg.name = ["fr_motor_joint"]
                     case 4:
                         motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_wheel_joint"]
+                        joint_state_msg.name = ["br_motor_joint"]
                     case _:
                         self.get_logger().warning(
                             f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
                         )
                         return
 
-                # make the fucking shit work
-                joint_state_msg.name.append("left_suspension_joint")
-                joint_state_msg.position.append(0.0)
-                joint_state_msg.velocity.append(0.0)
-
                 joint_state_msg.header.stamp = msg.header.stamp
                 self.joint_state_pub_.publish(joint_state_msg)
             case _:
@@ -629,10 +581,6 @@ def map_range(
     return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
-def radps_to_rpm(radps: float):
-    return radps * 60 / (2 * pi)
-
-
 def main(args=None):
     rclpy.init(args=args)
     sys.excepthook = myexcepthook