refactor: (arm) cleanup old standalone serial code

Removed literally 100 lines

README.md

@@ -16,6 +16,9 @@ You will use these packages to launch all rover-side ROS2 nodes.
   - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
 - [Common Problems/Toubleshooting](#common-problemstroubleshooting)
 - [Packages](#packages)
+- [Graphs](#graphs)
+  - [Full System](#full-system)
+  - [Individual Nodes](#individual-nodes)
 - [Maintainers](#maintainers)
 
 ## Software Prerequisites
@@ -65,10 +68,10 @@ You can fake the presence of a Serial device (i.e., MCU) by using the following
 $ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
 ```
 
-When you go to run anchor, use the `port_override` launch parameter to point it to the fake serial port, like so:
+When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
 
 ```bash
-$ ros2 launch anchor_pkg rover.launch.py port_override:=/tmp/ttyACM9
+$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
 ```
 
 ### Connecting the GuliKit Controller
@@ -121,26 +124,59 @@ A: Don't worry about it. If you had the workspace sourced, ROS2 will complain ab
 ...
 ```
 
-A: To find a microcontroller to talk to, Anchor filters through your available serial ports to find microcontrollers. If the microcontroller fails
-to respond properly, or one is not found, it will abort. There are a few possible fixes:
+A: To find a microcontroller to talk to, Anchor sends a ping to every Serial port on your computer. If it does not receive a 'pong' in less than one second, then it aborts. There are a few possible fixes:
 
 - Keep trying to run it until it works
 - Run `lsusb` to see if the microcontroller is detected by your computer.
 - Run `ls /dev/tty*0` to see if there is a valid Serial port enumerated for the microcontroller.
-- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`) by using the `groups` command.
+- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`).
 
 ## Packages
 
 - [anchor\_pkg](./src/anchor_pkg) - Handles Serial communication between the various other packages here and the microcontroller.
 - [arm\_pkg](./src/arm_pkg) - Relays controls and sensor data for the arm (socket and digit) between anchor and basestation/headless.
 - [astra\_descriptions](./src/astra_descriptions) - Submodule with URDF-related packages.
-- [astra\_msgs](./src/astra_msgs) - Contains custom message types for communication between basestation and the rover over ROS2.
 - [bio\_pkg](./src/bio_pkg) - Like arm_pkg, but for CITADEL and FAERIE
 - [core\_pkg](./src/core_pkg) - Like arm_pkg, but for Core
 - [headless\_pkg](./src/headless_pkg) - Simple, non-graphical controller node to work in place of basestation when controlling the rover by itself. This is autostarted with anchor to allow for setup-less control of the rover.
 - [latency\_tester](./src/latency_tester) - A temporary node to test comms latency over ROS2, Serial, and CAN.
+- [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
 - [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
 
+## Graphs
+
+### Full System
+
+> **Anchor stand-alone** (`ros2 launch anchor_pkg rover.launch.py`)
+>
+> ![rqt_graph of Anchor by itself, ran with command: ros2 launch anchor_pkg rover.launch.py](./docs-resources/graph-anchor-standalone.png)
+
+> **Anchor with [basestation-classic](https://github.com/SHC-ASTRA/basestation-classic)**
+>
+> ![rqt_graph of Anchor ran with the same command as above, talking to basestation-classic](./docs-resources/graph-anchor-w-basestation-classic.png)
+
+> **Anchor with Headless** (`ros2 run headless_pkg headless_full`)
+>
+> ![rqt_graph of Anchor ran with Headless](./docs-resources/graph-anchor-w-headless.png)
+
+### Individual Nodes
+
+> **Anchor** (`ros2 run anchor_pkg anchor`)
+>
+> ![rqt_graph of Anchor node running by itself](./docs-resources/graph-anchor-anchor-standalone.png)
+
+> **Core** (`ros2 run core_pkg core --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Core node running by itself](./docs-resources/graph-anchor-core-standalone.png)
+
+> **Arm** (`ros2 run arm_pkg arm --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Arm node running by itself](./docs-resources/graph-anchor-arm-standalone.png)
+
+> **Bio** (`ros2 run bio_pkg bio --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Bio node running by itself](./docs-resources/graph-anchor-bio-standalone.png)
+
 ## Maintainers
 
 | Name | Email | Discord |

flake.lock

@@ -24,27 +24,27 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1761810010,
-        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
+        "lastModified": 1770108954,
+        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
+        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "master",
+        "ref": "develop",
         "repo": "nix-ros-overlay",
         "type": "github"
       }
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1744849697,
-        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
-        "owner": "lopsided98",
+        "lastModified": 1759381078,
+        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
+        "owner": "NixOS",
         "repo": "nixpkgs",
-        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
+        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
         "type": "github"
       },
       "original": {

flake.nix

@@ -2,7 +2,7 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
   };
 
@@ -56,10 +56,12 @@
                   control-msgs
                   control-toolbox
                   moveit-core
+                  moveit-planners
                   moveit-common
                   moveit-msgs
                   moveit-ros-planning
                   moveit-ros-planning-interface
+		  moveit-ros-visualization
                   moveit-configs-utils
                   moveit-ros-move-group
                   moveit-servo
@@ -68,9 +70,9 @@
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
-                  chomp-motion-planner
                   joy
-                  # ros2-controllers nixpkg does not build :(
+                  ros2-controllers
+		  chomp-motion-planner
                 ];
               }
             )

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,17 +1,20 @@
-import atexit
-import glob
-import signal
-import threading
-from typing import cast
-
 import rclpy
+from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
+from std_srvs.srv import Empty
+
+import signal
+import time
+import atexit
+
 import serial
 import serial.tools.list_ports
-from rcl_interfaces.msg import ParameterDescriptor, ParameterType
-from rclpy.executors import ExternalShutdownException
-from rclpy.node import Node
-from std_msgs.msg import Header, String
+import os
+import sys
+import threading
+import glob
 
+from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
 
 KNOWN_USBS = [
@@ -43,23 +46,15 @@ class Anchor(Node):
         - Publish raw strings to this topic to send directly to the MCU for debugging
     """
 
-    serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
-
     def __init__(self):
         # Initalize node with name
-        super().__init__("anchor_node")
+        super().__init__("anchor_node")  # previously 'serial_publisher'
 
-        self.declare_parameter(
-            "port_override",
-            "",
-            ParameterDescriptor(
-                name="port_override", type=ParameterType.PARAMETER_STRING
-            ),
-        )
+        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
 
         # Serial port override
-        if port_override := self.get_parameter("port_override").value:
-            self.serial_port = cast(str, port_override)  # Cast to make the linter happy
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.serial_port = port_override
 
         ##################################################
         # Serial MCU Discovery
@@ -67,64 +62,104 @@ class Anchor(Node):
         # If there was not a port override, look for a MCU over USB for Serial.
         if self.serial_port is None:
             comports = serial.tools.list_ports.comports()
-            recog_ports = list(
+            real_ports = list(
                 filter(
-                    # Filter for ports we know that aren't invalid
-                    lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None,
+                    lambda p: p.vid is not None
+                    and p.pid is not None
+                    and p.device is not None,
                     comports,
                 )
             )
+            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
 
-            # Guards
-            if len(recog_ports) > 1:  # If we found too many
-                self.get_logger().fatal(
+            if len(recog_ports) == 1:  # Found singular recognized MCU
+                found_port = recog_ports[0]
+                self.get_logger().info(
+                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
+                )
+                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
+            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
+                # Kinda jank log message
+                self.get_logger().error(
                     f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
                 )
-                exit(1)
-            if len(recog_ports) == 0:
-                self.get_logger().fatal(
-                    f"Found no recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+                # Don't set self.serial_port; later if-statement will exit()
+            elif (
+                len(recog_ports) == 0 and len(real_ports) > 0
+            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
+                self.get_logger().error(
+                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
                 )
-                exit(1)
+                # Don't set self.serial_port; later if-statement will exit()
+            else:  # Found jack shit
+                self.get_logger().error("No valid Serial ports specified or found.")
+                # Don't set self.serial_port; later if-statement will exit()
 
-            # Everything is ok
-            found_port = recog_ports[0]
-            self.get_logger().info(
-                f"Selecting MCU '{found_port.description}' at {found_port.device}."
-            )
-            # String, location of device file; e.g., '/dev/ttyACM0'
-            self.serial_port = found_port.device
-
-        # Found a Serial port, try to open it; above code has not officially opened a Serial port
-        self.get_logger().debug(
-            f"Attempting to open Serial port '{self.serial_port}'..."
-        )
-        try:
-            self.serial_interface = serial.Serial(self.serial_port, 115200, timeout=1)
-
-            # Attempt to get name of connected MCU
-            self.serial_interface.write(b"can_relay_mode,on\n")  # can_relay_ready,[mcu]
-            mcu_name: str = ""
-            for _ in range(4):  # Sometimes it takes a sec
-                response = self.serial_interface.read_until(bytes("\n", "utf8"))
-                if b"can_relay_ready" in response:
-                    try:
-                        args: list[str] = response.decode("utf8").strip().split(",")
-                    except UnicodeDecodeError:
-                        continue  # ignore malformed responses
-                    if len(args) == 2:
-                        mcu_name = args[1]
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
+        # Loop through all serial devices on the computer to check for the MCU
+        if self.serial_port is None:
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
+            ports = Anchor.list_serial_ports()
+            for _ in range(4):
+                if self.serial_port is not None:
                     break
-            self.get_logger().info(
-                f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
-            )
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        # (f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until(bytes("\n", "utf8"))
 
-        except serial.SerialException as e:
-            self.get_logger().fatal(
-                f"Could not open Serial port '{self.serial_port}' for reason:"
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.serial_port = port
+                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                            break
+                    except:
+                        pass
+
+        # If port is still None then we ain't finding no mcu
+        if self.serial_port is None:
+            self.get_logger().error("Unable to find MCU. Exiting...")
+            time.sleep(1)
+            sys.exit(1)
+        # Found a Serial port, try to open it; above code has not officially opened a Serial port
+        else:
+            self.get_logger().debug(
+                f"Attempting to open Serial port '{self.serial_port}'..."
             )
-            self.get_logger().fatal(e.strerror)
-            exit(1)
+            try:
+                self.serial_interface = serial.Serial(
+                    self.serial_port, 115200, timeout=1
+                )
+
+                # Attempt to get name of connected MCU
+                self.serial_interface.write(
+                    b"can_relay_mode,on\n"
+                )  # can_relay_ready,[mcu]
+                mcu_name: str = ""
+                for _ in range(4):
+                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
+                    try:
+                        if b"can_relay_ready" in response:
+                            args: list[str] = response.decode("utf8").strip().split(",")
+                            if len(args) == 2:
+                                mcu_name = args[1]
+                            break
+                    except UnicodeDecodeError:
+                        pass  # ignore malformed responses
+                self.get_logger().info(
+                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                )
+
+            except serial.SerialException as e:
+                self.get_logger().error(
+                    f"Could not open Serial port '{self.serial_port}' for reason:"
+                )
+                self.get_logger().error(e.strerror)
+                time.sleep(1)
+                sys.exit(1)
 
         # Close serial port on exit
         atexit.register(self.cleanup)
@@ -170,38 +205,54 @@ class Anchor(Node):
 
     def read_MCU(self):
         """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
-        output: str | None = None
         try:
             output = str(self.serial_interface.readline(), "utf8")
+
+            if output:
+                self.relay_fromvic(output)
+                # All output over debug temporarily
+                # self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+                if output.startswith("can_relay_fromvic,core"):
+                    self.core_pub.publish(msg)
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for voltage readings
+                    self.arm_pub.publish(msg)
+                if 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
+                # self.debug_pub.publish(msg)
+                return
         except serial.SerialException as e:
-            self.get_logger().fatal(f"SerialException: {e}")
+            print(f"SerialException: {e}")
+            print("Closing serial port.")
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except TypeError as e:
-            self.get_logger().fatal(f"TypeError: {e}")
+            print(f"TypeError: {e}")
+            print("Closing serial port.")
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except Exception as e:
-            self.get_logger().error(f"Exception: {e}")
-
-        if output:
-            self.relay_fromvic(output)
-            msg = String()
-            msg.data = output
-
-            # All output over debug
-            self.debug_pub.publish(msg)
-
-            # Send the message to the right place
-            if output.startswith("can_relay_fromvic,core"):
-                self.core_pub.publish(msg)
-            elif output.startswith("can_relay_fromvic,arm") or output.startswith(
-                "can_relay_fromvic,digit"
-            ):  # digit for voltage readings
-                self.arm_pub.publish(msg)
-            if output.startswith("can_relay_fromvic,citadel") or output.startswith(
-                "can_relay_fromvic,digit"
-            ):  # digit for SHT sensor
-                self.bio_pub.publish(msg)
-            return
+            print(f"Exception: {e}")
+            # print("Closing serial port.")
+            # if self.ser.is_open:
+            #     self.ser.close()
+            # exit(1)
 
     def on_mock_fromvic(self, msg: String):
         """For testing without an actual MCU, publish strings here as if they came from an MCU"""
@@ -309,6 +360,6 @@ def main(args=None):
 
 if __name__ == "__main__":
     signal.signal(
-        signal.SIGTERM, lambda signum, frame: exit(0)
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
     )  # Catch termination signals and exit cleanly
     main()

src/anchor_pkg/launch/rover.launch.py

@@ -8,6 +8,7 @@ from launch.substitutions import (
     PathJoinSubstitution,
 )
 from launch_ros.actions import Node
+from launch.conditions import IfCondition
 
 
 # Prevent making __pycache__ directories
@@ -50,6 +51,7 @@ def launch_setup(context, *args, **kwargs):
                 executable="ptz",  # change as needed
                 name="ptz",
                 output="both",
+                condition=IfCondition(LaunchConfiguration("use_ptz", default="true")),
                 # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
                 # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
             )

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy import qos
 import serial
 import sys
 import threading
@@ -7,11 +8,10 @@ import glob
 import time
 import atexit
 import signal
-from std_msgs.msg import String
-from astra_msgs.msg import ArmManual
-from astra_msgs.msg import SocketFeedback
-from astra_msgs.msg import DigitFeedback
+from std_msgs.msg import String, Header
 from sensor_msgs.msg import JointState
+from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual
+from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
 import math
 
 # control_qos = qos.QoSProfile(
@@ -25,37 +25,68 @@ import math
 #     liveliness_lease_duration=5000
 # )
 
-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_socket_msg_len_dict = {
+    53: 4,
+    54: 4,
+    55: 4,
+    58: 4,
+    59: 4,
+}
+viccan_digit_msg_len_dict = {
+    54: 4,
+    55: 2,
+    59: 2,
+}
 
-class SerialRelay(Node):
+
+class ArmNode(Node):
     def __init__(self):
-        # Initialize node
         super().__init__("arm_node")
 
-        # Get launch mode parameter
-        self.declare_parameter("launch_mode", "arm")
-        self.launch_mode = self.get_parameter("launch_mode").value
-        self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
+        self.get_logger().info(f"arm launch_mode is: anchor")  # Hey I like the output
 
-        # Create publishers
-        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
-        self.socket_pub = self.create_publisher(
-            SocketFeedback, "/arm/feedback/socket", 10
+        ##################################################
+        # Topics
+
+        # Anchor topics
+        self.anchor_fromvic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20
         )
-        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
-        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
-
-        # Create subscribers
-        self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 10
+        self.anchor_tovic_pub_ = self.create_publisher(
+            VicCAN, "/anchor/to_vic/relay", 20
         )
 
-        # New messages
-        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
-        self.joint_state = JointState()
-        self.joint_state.name = [
+        self.anchor_sub = self.create_subscription(
+            String, "/anchor/arm/feedback", self.anchor_feedback, 10
+        )
+        self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+
+        # Control
+
+        # Manual: who tf knows. Maybe JointJog?
+        # IK: /joint_commands is published by JointTrajectoryController via topic_based_control
+        self.joint_command_sub_ = self.create_subscription(
+            JointState, "/joint_commands", self.joint_command_callback, 1
+        )
+
+        # Feedback
+
+        self.arm_feedback_pub_ = self.create_publisher(
+            ArmFeedback,
+            "/arm/feedback/new_feedback",
+            qos_profile=qos.qos_profile_sensor_data,
+        )
+        self.arm_feedback_new = ArmFeedback()
+        # IK: /joint_states is published from here to topic_based_control
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
+        self.saved_joint_state = JointState()
+        self.saved_joint_state.name = [
             "Axis_0_Joint",
             "Axis_1_Joint",
             "Axis_2_Joint",
@@ -64,117 +95,63 @@ class SerialRelay(Node):
             "Wrist-EF_Roll_Joint",
             "Gripper_Slider_Left",
         ]
-        self.joint_state.position = [0.0] * len(
-            self.joint_state.name
+        self.saved_joint_state.position = [0.0] * len(
+            self.saved_joint_state.name
+        )  # Initialize with zeros
+        self.saved_joint_state.velocity = [0.0] * len(
+            self.saved_joint_state.name
         )  # Initialize with zeros
 
-        self.joint_command_sub = self.create_subscription(
-            JointState, "/joint_commands", self.joint_command_callback, 10
+        # Old
+
+        # Create publishers
+        self.socket_pub = self.create_publisher(
+            SocketFeedback, "/arm/feedback/socket", 10
         )
-
-        # Topics used in anchor mode
-        if self.launch_mode == "anchor":
-            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 = SocketFeedback()
+        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
         self.digit_feedback = DigitFeedback()
+        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
 
-        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == "arm":
-            # Loop through all serial devices on the computer to check for the MCU
-            self.port = None
-            ports = SerialRelay.list_serial_ports()
-            for _ in range(4):
-                for port in ports:
-                    try:
-                        # connect and send a ping command
-                        ser = serial.Serial(port, 115200, timeout=1)
-                        # print(f"Checking port {port}...")
-                        ser.write(b"ping\n")
-                        response = ser.read_until("\n")  # type: ignore
-
-                        # if pong is in response, then we are talking with the MCU
-                        if b"pong" in response:
-                            self.port = port
-                            self.get_logger().info(f"Found MCU at {self.port}!")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
-
-            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)
-            atexit.register(self.cleanup)
+        # Create subscribers
+        self.man_sub = self.create_subscription(
+            ArmManual, "/arm/control/manual", self.send_manual, 10
+        )
 
     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 == "arm":
-                    if self.ser.in_waiting:
-                        self.read_mcu()
-                    else:
-                        time.sleep(0.1)
+                pass
         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 joint_command_callback(self, msg: JointState):
-        # Embedded takes deg*10, ROS2 uses Radians
-        positions = [math.degrees(pos) * 10 for pos in msg.position]
-        # Axis 2 & 3 URDF direction is inverted
-        positions[2] = -positions[2]
-        positions[3] = -positions[3]
+        if len(msg.position) < 7 and len(msg.velocity) < 7:
+            return  # command needs either position or velocity for all 7 joints
 
-        # Set target angles for each arm axis for embedded IK PID to handle
-        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
-        # Wrist yaw and roll
-        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
-        # Gripper IK does not have adequate hardware yet
-        self.send_cmd(command)
+        # Assumed order: Axis0, Axis1, Axis2, Axis3, Wrist_Yaw, Wrist_Roll, Gripper
+        # TODO: formalize joint names in URDF, refactor here to depend on joint names
+        # Embedded takes deg*10, ROS2 uses Radians
+        velocities = [
+            math.degrees(vel) * 10 if abs(vel) > 0.05 else 0.0 for vel in msg.velocity
+        ]
+        # Axis 2 & 3 URDF direction is inverted
+        velocities[2] = -velocities[2]
+        velocities[3] = -velocities[3]
+
+        # Axis 0-3
+        arm_cmd = VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3])
+        arm_cmd.header = Header(stamp=self.get_clock().now().to_msg())
+        # Wrist yaw and roll, gripper included for future use when have adequate hardware
+        digit_cmd = VicCAN(mcu_name="digit", command_id=43, data=velocities[4:6])
+        digit_cmd.header = Header(stamp=self.get_clock().now().to_msg())
+
+        self.anchor_tovic_pub_.publish(arm_cmd)
+        self.anchor_tovic_pub_.publish(digit_cmd)
 
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
@@ -201,23 +178,14 @@ class SerialRelay(Node):
         return
 
     def send_cmd(self, msg: str):
-        if (
-            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 == "arm":  # if in standalone mode, send to MCU directly
-            self.get_logger().info(f"[Arm to MCU] {msg}")
-            self.ser.write(bytes(msg, "utf8"))
+        output = String(data=msg)
+        self.anchor_pub.publish(output)
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
-            # pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
-            # pass
             self.updateBusVoltage(output)
         elif output.startswith("can_relay_fromvic,arm,53"):
             self.updateMotorFeedback(output)
@@ -235,15 +203,132 @@ class SerialRelay(Node):
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
                 # self.digit_feedback.wrist_roll = float(parts[4])
-                self.joint_state.position[4] = math.radians(
-                    float(parts[4])
-                )  # Wrist roll
-                self.joint_state.position[5] = math.radians(
-                    float(parts[3])
-                )  # Wrist yaw
         else:
             return
 
+    def relay_fromvic(self, msg: VicCAN):
+        # Code for socket and digit are broken out for cleaner code
+        if msg.mcu_name == "arm":
+            self.process_fromvic_arm(msg)
+        elif msg.mcu_name == "digit":
+            self.process_fromvic_digit(msg)
+
+    def process_fromvic_arm(self, msg: VicCAN):
+        if msg.mcu_name != "arm":
+            return
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_socket_msg_len_dict:
+            expected_len = viccan_socket_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
+
+        match msg.command_id:
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.temperature = float(msg.data[1]) / 10.0
+                    motor.voltage = float(msg.data[2]) / 10.0
+                    motor.current = float(msg.data[3]) / 10.0
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 54:  # Board voltages
+                self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.socket_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.socket_voltage.v5 = float(msg.data[2]) / 100.0
+                self.arm_feedback_new.socket_voltage.v3 = float(msg.data[3]) / 100.0
+            case 55:  # Arm joint positions
+                angles = [angle / 10.0 for angle in msg.data]  # VicCAN sends deg*10
+                # Joint state publisher for URDF visualization
+                self.saved_joint_state.position[0] = math.radians(angles[0])  # Axis 0
+                self.saved_joint_state.position[1] = math.radians(angles[1])  # Axis 1
+                self.saved_joint_state.position[2] = math.radians(
+                    -angles[2]
+                )  # Axis 2 (inverted)
+                self.saved_joint_state.position[3] = math.radians(
+                    -angles[3]
+                )  # Axis 3 (inverted)
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+            case 58:  # REV SPARK MAX position and velocity feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.position = float(msg.data[1])
+                    motor.velocity = float(msg.data[2])
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 59:  # Arm joint velocities
+                velocities = [vel / 100.0 for vel in msg.data]  # VicCAN sends deg/s*100
+                self.saved_joint_state.velocity[0] = math.radians(
+                    velocities[0]
+                )  # Axis 0
+                self.saved_joint_state.velocity[1] = math.radians(
+                    velocities[1]
+                )  # Axis 1
+                self.saved_joint_state.velocity[2] = math.radians(
+                    -velocities[2]
+                )  # Axis 2 (-)
+                self.saved_joint_state.velocity[3] = math.radians(
+                    -velocities[3]
+                )  # Axis 3 (-)
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+
+    def process_fromvic_digit(self, msg: VicCAN):
+        if msg.mcu_name != "digit":
+            return
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_digit_msg_len_dict:
+            expected_len = viccan_digit_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
+
+        match msg.command_id:
+            case 54:  # Board voltages
+                self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
+            case 55:  # Arm joint positions
+                self.saved_joint_state.position[4] = math.radians(
+                    msg.data[0]
+                )  # Wrist roll
+                self.saved_joint_state.position[5] = math.radians(
+                    msg.data[1]
+                )  # Wrist yaw
+
     def publish_feedback(self):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
@@ -263,16 +348,6 @@ class SerialRelay(Node):
             self.arm_feedback.axis1_angle = angles[1]
             self.arm_feedback.axis2_angle = angles[2]
             self.arm_feedback.axis3_angle = angles[3]
-
-            # Joint state publisher for URDF visualization
-            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
-            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
-            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
-            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
-            # Wrist is handled by digit feedback
-            self.joint_state.header.stamp = self.get_clock().now().to_msg()
-            self.joint_state_pub.publish(self.joint_state)
-
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
@@ -313,33 +388,13 @@ class SerialRelay(Node):
             self.arm_feedback.axis0_voltage = voltage
             self.arm_feedback.axis0_current = current
 
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        # return glob.glob("/dev/tty[A-Za-z]*")
-
-    def cleanup(self):
-        print("Cleaning up...")
-        try:
-            if self.ser.is_open:
-                self.ser.close()
-        except Exception as e:
-            exit(0)
-
-
-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
 
-    global serial_pub
-    serial_pub = SerialRelay()
-    serial_pub.run()
+    arm_node = ArmNode()
+    arm_node.run()
+    rclpy.try_shutdown()
 
 
 if __name__ == "__main__":

src/core_pkg/core_pkg/core_ptz.py

@@ -1,25 +1,24 @@
 #!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
 import asyncio
+from concurrent.futures import ThreadPoolExecutor
 import signal
 import sys
 import threading
 import time
-from concurrent.futures import ThreadPoolExecutor
 
-import rclpy
-from rclpy.node import Node
 from std_msgs.msg import String
-
 from astra_msgs.msg import PtzControl, PtzFeedback
 
 # Import the SIYI SDK
 from core_pkg.siyi_sdk import (
-    AttitudeData,
-    CommandID,
-    DataStreamFrequency,
-    DataStreamType,
-    SingleAxis,
     SiyiGimbalCamera,
+    CommandID,
+    DataStreamType,
+    DataStreamFrequency,
+    SingleAxis,
+    AttitudeData,
 )
 
 
@@ -263,7 +262,7 @@ class PtzNode(Node):
             f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
         )
         self.debug_pub.publish(msg)
-        self.get_logger().debug(message_text)
+        self.get_logger().info(message_text)
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""

src/headless_pkg/src/headless_node.py

@@ -16,8 +16,10 @@ import pwd
 import grp
 from math import copysign
 
+from std_srvs.srv import Trigger
 from std_msgs.msg import String
-from geometry_msgs.msg import Twist
+from geometry_msgs.msg import Twist, TwistStamped
+from control_msgs.msg import JointJog
 from astra_msgs.msg import CoreControl, ArmManual, BioControl
 from astra_msgs.msg import CoreCtrlState
 
@@ -45,8 +47,17 @@ control_qos = qos.QoSProfile(
     liveliness_lease_duration=Duration(seconds=5),
 )
 
+arm_ik_qos = qos.QoSProfile(
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
+)
+
 CORE_MODE = "twist"  # "twist" or "duty"
 
+STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05"))
+
 
 class Headless(Node):
     def __init__(self):
@@ -105,10 +116,16 @@ class Headless(Node):
                 break
             id += 1
 
-        self.create_timer(0.15, self.send_controls)
+        self.create_timer(0.1, self.send_controls)
 
         self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
         self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
+        self.arm_ik_twist_publisher = self.create_publisher(
+            TwistStamped, "/servo_node/delta_twist_cmds", arm_ik_qos
+        )
+        self.arm_ik_jointjog_publisher = self.create_publisher(
+            JointJog, "/servo_node/delta_joint_cmds", arm_ik_qos
+        )
         self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
 
         self.core_twist_pub_ = self.create_publisher(
@@ -118,6 +135,41 @@ class Headless(Node):
             CoreCtrlState, "/core/control/state", qos_profile=control_qos
         )
 
+        self.declare_parameter("arm_mode", "manual")
+        self.arm_mode = self.get_parameter("arm_mode").value
+
+        self.declare_parameter("arm_manual_scheme", "old")
+        self.arm_manual_scheme = self.get_parameter("arm_manual_scheme").value
+
+        # Check parameter validity
+        if self.arm_mode not in ["manual", "ik"]:
+            self.get_logger().warn(
+                f"Invalid value '{self.arm_mode}' for arm_mode parameter. Defaulting to 'manual' (per-axis control)."
+            )
+        if self.arm_manual_scheme not in ["old", "new"]:
+            self.get_logger().warn(
+                f"Invalid value '{self.arm_manual_scheme}' for arm_manual_scheme parameter. Defaulting to 'old' ('24 and '25 controls)."
+            )
+
+        # If using IK control, we have to "start" the servo node to enable it to accept commands
+        self.servo_start_client = None
+        if self.arm_mode == "ik":
+            self.get_logger().info("Starting servo node for IK control...")
+            self.servo_start_client = self.create_client(
+                Trigger, "/servo_node/start_servo"
+            )
+            timeout_counter = 0
+            while not self.servo_start_client.wait_for_service(timeout_sec=1.0):
+                self.get_logger().info("Waiting for servo_node/start_servo service...")
+                timeout_counter += 1
+                if timeout_counter >= 10:
+                    self.get_logger().error(
+                        "Servo's start service not available. IK control will not work."
+                    )
+                    break
+            if self.servo_start_client.service_is_ready():
+                self.servo_start_client.call_async(Trigger.Request())
+
         self.ctrl_mode = "core"  # Start in core mode
         self.core_brake_mode = False
         self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
@@ -125,18 +177,6 @@ class Headless(Node):
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
-    def run(self):
-        # This thread makes all the update processes run in the background
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                self.send_controls()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
-
     def send_controls(self):
         """Read the gamepad state and publish control messages"""
         for event in pygame.event.get():
@@ -166,6 +206,9 @@ class Headless(Node):
             new_ctrl_mode = "core"
 
         if new_ctrl_mode != self.ctrl_mode:
+            self.core_publisher.publish(CORE_STOP_MSG)
+            self.arm_publisher.publish(ARM_STOP_MSG)
+            self.bio_publisher.publish(BIO_STOP_MSG)
             self.gamepad.rumble(0.6, 0.7, 75)
             self.ctrl_mode = new_ctrl_mode
             self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
@@ -194,8 +237,6 @@ class Headless(Node):
             self.get_logger().info(f"[Ctrl] {output}")
 
             self.core_publisher.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
 
         elif self.ctrl_mode == "core" and CORE_MODE == "twist":
             input = Twist()
@@ -215,8 +256,6 @@ class Headless(Node):
 
             # Publish
             self.core_twist_pub_.publish(input)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            # self.bio_publisher.publish(BIO_STOP_MSG)
             self.get_logger().info(
                 f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
             )
@@ -247,7 +286,7 @@ class Headless(Node):
                 )
 
         # ARM and BIO
-        if self.ctrl_mode == "arm":
+        if self.ctrl_mode == "arm" and self.arm_mode == "manual":
             arm_input = ArmManual()
 
             # Collect controller state
@@ -257,50 +296,114 @@ class Headless(Node):
             right_stick_x = deadzone(self.gamepad.get_axis(3))
             right_stick_y = deadzone(self.gamepad.get_axis(4))
             right_trigger = deadzone(self.gamepad.get_axis(5))
+            button_a = self.gamepad.get_button(0)
+            button_b = self.gamepad.get_button(1)
+            button_x = self.gamepad.get_button(2)
+            button_y = self.gamepad.get_button(3)
+            left_bumper = self.gamepad.get_button(4)
             right_bumper = self.gamepad.get_button(5)
             dpad_input = self.gamepad.get_hat(0)
 
-            # EF Grippers
-            if left_trigger > 0 and right_trigger > 0:
-                arm_input.gripper = 0
-            elif left_trigger > 0:
-                arm_input.gripper = -1
-            elif right_trigger > 0:
-                arm_input.gripper = 1
+            if self.arm_manual_scheme == "old":
+                # EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
 
-            # Axis 0
-            if dpad_input[0] == 1:
-                arm_input.axis0 = 1
-            elif dpad_input[0] == -1:
-                arm_input.axis0 = -1
+                # Axis 0
+                if dpad_input[0] == 1:
+                    arm_input.axis0 = 1
+                elif dpad_input[0] == -1:
+                    arm_input.axis0 = -1
 
-            if right_bumper:  # Control end effector
+                if right_bumper:  # Control end effector
 
-                # Effector yaw
-                if left_stick_x > 0:
-                    arm_input.effector_yaw = 1
-                elif left_stick_x < 0:
-                    arm_input.effector_yaw = -1
+                    # Effector yaw
+                    if left_stick_x > 0:
+                        arm_input.effector_yaw = 1
+                    elif left_stick_x < 0:
+                        arm_input.effector_yaw = -1
 
-                # Effector roll
-                if right_stick_x > 0:
-                    arm_input.effector_roll = 1
-                elif right_stick_x < 0:
+                    # Effector roll
+                    if right_stick_x > 0:
+                        arm_input.effector_roll = 1
+                    elif right_stick_x < 0:
+                        arm_input.effector_roll = -1
+
+                else:  # Control arm axis
+
+                    # Axis 1
+                    if abs(left_stick_x) > 0.15:
+                        arm_input.axis1 = round(left_stick_x)
+
+                    # Axis 2
+                    if abs(left_stick_y) > 0.15:
+                        arm_input.axis2 = -1 * round(left_stick_y)
+
+                    # Axis 3
+                    if abs(right_stick_y) > 0.15:
+                        arm_input.axis3 = -1 * round(right_stick_y)
+
+            if self.arm_manual_scheme == "new":
+                # Right stick: EF yaw and axis 3
+                # Left stick: axis 1 and 2
+                # D-pad: axis 0 and _
+                # Triggers: EF grippers
+                # Bumpers: EF roll
+                # A: brake
+                # B: _
+                # X: _
+                # Y: linear actuator
+
+                ARM_THRESHOLD = 0.2
+
+                # Right stick: EF yaw and axis 3
+                arm_input.effector_yaw = (
+                    0 if abs(right_stick_x) < ARM_THRESHOLD else int(copysign(1, right_stick_x))
+                )
+                arm_input.axis3 = (
+                    0 if abs(right_stick_y) < ARM_THRESHOLD else int(-1 * copysign(1, right_stick_y))
+                )
+
+                # Left stick: axis 1 and 2
+                arm_input.axis1 = (
+                    0 if abs(left_stick_x) < ARM_THRESHOLD else int(copysign(1, left_stick_x))
+                )
+                arm_input.axis2 = (
+                    0 if abs(left_stick_y) < ARM_THRESHOLD else int(-1 * copysign(1, left_stick_y))
+                )
+
+                # D-pad: axis 0 and _
+                arm_input.axis0 = (
+                    0 if dpad_input[0] == 0 else int(copysign(1, dpad_input[0]))
+                )
+
+                # Triggers: EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
+
+                # Bumpers: EF roll
+                if left_bumper > 0 and right_bumper > 0:
+                    arm_input.effector_roll = 0
+                elif left_bumper > 0:
                     arm_input.effector_roll = -1
+                elif right_bumper > 0:
+                    arm_input.effector_roll = 1
 
-            else:  # Control arm axis
+                # A: brake
+                if button_a:
+                    arm_input.brake = True
 
-                # Axis 1
-                if abs(left_stick_x) > 0.15:
-                    arm_input.axis1 = round(left_stick_x)
-
-                # Axis 2
-                if abs(left_stick_y) > 0.15:
-                    arm_input.axis2 = -1 * round(left_stick_y)
-
-                # Axis 3
-                if abs(right_stick_y) > 0.15:
-                    arm_input.axis3 = -1 * round(right_stick_y)
+                # Y: linear actuator
+                if button_y:
+                    arm_input.linear_actuator = 1
 
             # BIO
             bio_input = BioControl(
@@ -314,12 +417,75 @@ class Headless(Node):
                     30 * (right_trigger - left_trigger)
                 )  # Max duty cycle 30%
 
-            self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(arm_input)
-            # self.bio_publisher.publish(bio_input)
+
+        if self.ctrl_mode == "arm" and self.arm_mode == "ik":
+            arm_twist = TwistStamped()
+            arm_twist.header.frame_id = "base_link"
+            arm_twist.header.stamp = self.get_clock().now().to_msg()
+            arm_jointjog = JointJog()
+            arm_jointjog.header.frame_id = "base_link"
+            arm_jointjog.header.stamp = self.get_clock().now().to_msg()
+
+            # Collect controller state
+            left_stick_x = deadzone(self.gamepad.get_axis(0))
+            left_stick_y = deadzone(self.gamepad.get_axis(1))
+            left_trigger = deadzone(self.gamepad.get_axis(2))
+            right_stick_x = deadzone(self.gamepad.get_axis(3))
+            right_stick_y = deadzone(self.gamepad.get_axis(4))
+            right_trigger = deadzone(self.gamepad.get_axis(5))
+            button_a = self.gamepad.get_button(0)
+            button_b = self.gamepad.get_button(1)
+            button_x = self.gamepad.get_button(2)
+            button_y = self.gamepad.get_button(3)
+            left_bumper = self.gamepad.get_button(4)
+            right_bumper = self.gamepad.get_button(5)
+            dpad_input = self.gamepad.get_hat(0)
+
+            # Right stick: linear y and linear x
+            # Left stick: angular z and linear z
+            # D-pad: angular y and _
+            # Triggers: EF grippers
+            # Bumpers: angular x
+            # A: brake
+            # B: IK mode
+            # X: manual mode
+            # Y: linear actuator
+
+            # Right stick: linear y and linear x
+            arm_twist.twist.linear.y = float(right_stick_x)
+            arm_twist.twist.linear.x = float(right_stick_y)
+
+            # Left stick: angular z and linear z
+            arm_twist.twist.angular.z = float(-1 * left_stick_x)
+            arm_twist.twist.linear.z = float(-1 * left_stick_y)
+            # D-pad: angular y and _
+            arm_twist.twist.angular.y = (
+                float(0)
+                if dpad_input[0] == 0
+                else float(-1 * copysign(0.75, dpad_input[0]))
+            )
+
+            # Triggers: EF Grippers
+            if left_trigger > 0 or right_trigger > 0:
+                arm_jointjog.joint_names.append(
+                    "Gripper_Slider_Left"  # TODO: Update joint name
+                )
+                arm_jointjog.velocities.append(float(right_trigger - left_trigger))
+
+            # Bumpers: angular x
+            if left_bumper > 0 and right_bumper > 0:
+                arm_twist.twist.angular.x = float(0)
+            elif left_bumper > 0:
+                arm_twist.twist.angular.x = float(1)
+            elif right_bumper > 0:
+                arm_twist.twist.angular.x = float(-1)
+
+            self.arm_ik_twist_publisher.publish(arm_twist)
+            # self.arm_ik_jointjog_publisher.publish(arm_jointjog)  # TODO: Figure this shit out
 
 
-def deadzone(value: float, threshold=0.05) -> float:
+def deadzone(value: float, threshold=STICK_DEADZONE) -> float:
     """Apply a deadzone to a joystick input so the motors don't sound angry"""
     if abs(value) < threshold:
         return 0
@@ -353,7 +519,7 @@ def main(args=None):
     except (KeyboardInterrupt, ExternalShutdownException):
         print("Caught shutdown signal. Exiting...")
     finally:
-        rclpy.shutdown()
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":

src/latency_tester/src/embedded_ping.cpp

@@ -26,7 +26,7 @@ class LatencyTester : public rclcpp::Node
 {
 public:
     LatencyTester()
-    : Node("latency_tester"), count_(0), target_mcu_("core")
+    : Node("latency_tester"), count_(0)
     {
         publisher_ = this->create_publisher<std_msgs::msg::String>("/anchor/relay", 10);
         timer_ = this->create_wall_timer(
@@ -35,6 +35,8 @@ public:
             "/anchor/debug",
             10,
             std::bind(&LatencyTester::response_callback, this, std::placeholders::_1));
+
+        target_mcu_ = this->declare_parameter<std::string>("target_mcu", "core");
     }
 
 private:

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -107,23 +107,23 @@ bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& but
                      std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
                      std::unique_ptr<control_msgs::msg::JointJog>& joint)
 {
-    // // Give joint jogging priority because it is only buttons
-    // // If any joint jog command is requested, we are only publishing joint commands
-    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
-    // {
-    //   // Map the D_PAD to the proximal joints
-    //   joint->joint_names.push_back("panda_joint1");
-    //   joint->velocities.push_back(axes[D_PAD_X]);
-    //   joint->joint_names.push_back("panda_joint2");
-    //   joint->velocities.push_back(axes[D_PAD_Y]);
+    // Give joint jogging priority because it is only buttons
+    // If any joint jog command is requested, we are only publishing joint commands
+    if (0)
+    {
+      // Map the D_PAD to the proximal joints
+      joint->joint_names.push_back("panda_joint1");
+      joint->velocities.push_back(axes[D_PAD_X]);
+      joint->joint_names.push_back("panda_joint2");
+      joint->velocities.push_back(axes[D_PAD_Y]);
 
-    //   // Map the diamond to the distal joints
-    //   joint->joint_names.push_back("panda_joint7");
-    //   joint->velocities.push_back(buttons[B] - buttons[X]);
-    //   joint->joint_names.push_back("panda_joint6");
-    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
-    //   return false;
-    // }
+      // Map the diamond to the distal joints
+      joint->joint_names.push_back("panda_joint7");
+      joint->velocities.push_back(buttons[B] - buttons[X]);
+      joint->joint_names.push_back("panda_joint6");
+      joint->velocities.push_back(buttons[Y] - buttons[A]);
+      return false;
+    }
 
     // The bread and butter: map buttons to twist commands
     twist->twist.linear.z = axes[RIGHT_STICK_Y];
@@ -243,13 +243,13 @@ public:
             twist_msg->header.stamp = this->now();
             twist_pub_->publish(std::move(twist_msg));
         }
-        // else
-        // {
-        //     // publish the JointJog
-        //     joint_msg->header.stamp = this->now();
-        //     joint_msg->header.frame_id = "panda_link3";
-        //     joint_pub_->publish(std::move(joint_msg));
-        // }
+        else
+        {
+            // publish the JointJog
+            joint_msg->header.stamp = this->now();
+            joint_msg->header.frame_id = "panda_link3";
+            joint_pub_->publish(std::move(joint_msg));
+        }
     }
 
 private: