feat: (core) make compatible with ros2_control

Qos disable

README.md

@@ -65,10 +65,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,24 +121,23 @@ 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
+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:
-to respond properly, or one is not found, it will abort. 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.
 
 ## Maintainers

flake.lock

@@ -24,27 +24,27 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1761810010,
+        "lastModified": 1770108954,
-        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
+        "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,
+        "lastModified": 1759381078,
-        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
+        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "lopsided98",
+        "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
+import os
-from rclpy.executors import ExternalShutdownException
+import sys
-from rclpy.node import Node
+import threading
-from std_msgs.msg import Header, String
+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(
+        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
-            "port_override",
-            "",
-            ParameterDescriptor(
-                name="port_override", type=ParameterType.PARAMETER_STRING
-            ),
-        )
 
         # Serial port override
-        if port_override := self.get_parameter("port_override").value:
+        if port_override := os.getenv("PORT_OVERRIDE"):
-            self.serial_port = cast(str, port_override)  # Cast to make the linter happy
+            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 is not None
-                    lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device 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:  # Found singular recognized MCU
-            if len(recog_ports) > 1:  # If we found too many
+                found_port = recog_ports[0]
-                self.get_logger().fatal(
+                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)
+                # Don't set self.serial_port; later if-statement will exit()
-            if len(recog_ports) == 0:
+            elif (
-                self.get_logger().fatal(
+                len(recog_ports) == 0 and len(real_ports) > 0
-                    f"Found no recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+            ):  # 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
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
-            found_port = recog_ports[0]
+        # Loop through all serial devices on the computer to check for the MCU
-            self.get_logger().info(
+        if self.serial_port is None:
-                f"Selecting MCU '{found_port.description}' at {found_port.device}."
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
-            )
+            ports = Anchor.list_serial_ports()
-            # String, location of device file; e.g., '/dev/ttyACM0'
+            for _ in range(4):
-            self.serial_port = found_port.device
+                if self.serial_port is not None:
-
-        # 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]
                     break
-            self.get_logger().info(
+                for port in ports:
-                f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                    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:
+                        # if pong is in response, then we are talking with the MCU
-            self.get_logger().fatal(
+                        if b"pong" in response:
-                f"Could not open Serial port '{self.serial_port}' for reason:"
+                            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)
+            try:
-            exit(1)
+                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}")
+            print(f"Exception: {e}")
-
+            # print("Closing serial port.")
-        if output:
+            # if self.ser.is_open:
-            self.relay_fromvic(output)
+            #     self.ser.close()
-            msg = String()
+            # exit(1)
-            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
 
     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

@@ -1,13 +1,21 @@
 #!/usr/bin/env python3
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.actions import (
+    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
@@ -40,7 +48,10 @@ def launch_setup(context, *args, **kwargs):
                 executable="core",  # change as needed
                 name="core",
                 output="both",
-                parameters=[{"launch_mode": mode}],
+                parameters=[
+                    {"launch_mode": mode},
+                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
+                ],
                 on_exit=Shutdown(),
             )
         )
@@ -133,4 +144,46 @@ def generate_launch_description():
         description="Launch mode: arm, core, bio, anchor, or ptz",
     )
 
-    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
+    ros2_control_arg = DeclareLaunchArgument(
+        "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/arm_pkg/arm_pkg/arm_node.py

@@ -49,7 +49,7 @@ class SerialRelay(Node):
 
         # Create subscribers
         self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 10
+            ArmManual, "/arm/control/manual", self.send_manual, 2
         )
 
         # New messages
@@ -190,7 +190,7 @@ class SerialRelay(Node):
 
         command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
 
-        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
+        command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
         command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 

src/core_pkg/core_pkg/core_node.py

@@ -31,14 +31,14 @@ CORE_WHEEL_RADIUS = 0.171  # meters
 CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
 
 control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
+    # deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5),
+    # liveliness_lease_duration=Duration(seconds=5),
 )
 
 # Used to verify the length of an incoming VicCAN feedback message
@@ -66,6 +66,10 @@ 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
 
@@ -85,24 +89,28 @@ class SerialRelay(Node):
 
         # Control
 
-        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
+        if self.use_ros2_control:
-        self.cmd_vel_sub_ = self.create_subscription(
+            # Joint state control for topic-based controller
-            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
+            self.joint_command_sub_ = self.create_subscription(
-        )
+                JointState, "/core/joint_commands", self.joint_command_callback, 2
-        # manual twist -- [-1, 1] rather than real units
+            )
-        self.twist_man_sub_ = self.create_subscription(
+        else:
-            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+            # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        )
+            self.cmd_vel_sub_ = self.create_subscription(
-        # manual flags -- brake mode and max duty cycle
+                TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
-        self.control_state_sub_ = self.create_subscription(
+            )
-            CoreCtrlState,
+            # manual twist -- [-1, 1] rather than real units
-            "/core/control/state",
+            self.twist_man_sub_ = self.create_subscription(
-            self.control_state_callback,
+                Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
-            qos_profile=control_qos,
+            )
-        )
+            # manual flags -- brake mode and max duty cycle
-        self.twist_max_duty = (
+            self.control_state_sub_ = self.create_subscription(
-            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+                CoreCtrlState,
-        )
+                "/core/control/state",
+                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
 
         # Feedback
 
@@ -122,7 +130,7 @@ class SerialRelay(Node):
         )  # TODO: not sure about this
         # Joint states for topic-based controller
         self.joint_state_pub_ = self.create_publisher(
-            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
         )
         # IMU (embedded BNO-055)
         self.imu_pub_ = self.create_publisher(
@@ -148,10 +156,11 @@ class SerialRelay(Node):
 
         # Old
 
-        # /core/control
+        if not self.use_ros2_control:
-        self.control_sub = self.create_subscription(
+            # /core/control
-            CoreControl, "/core/control", self.send_controls, 10
+            self.control_sub = self.create_subscription(
-        )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
+                CoreControl, "/core/control", self.send_controls, 10
+            )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
         # /core/feedback
         self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
         self.core_feedback = CoreFeedback()
@@ -283,6 +292,40 @@ 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
@@ -528,22 +571,27 @@ class SerialRelay(Node):
                 match motorId:
                     case 1:
                         motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_motor_joint"]
+                        joint_state_msg.name = ["fl_wheel_joint"]
                     case 2:
                         motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_motor_joint"]
+                        joint_state_msg.name = ["bl_wheel_joint"]
                     case 3:
                         motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_motor_joint"]
+                        joint_state_msg.name = ["fr_wheel_joint"]
                     case 4:
                         motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_motor_joint"]
+                        joint_state_msg.name = ["br_wheel_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 _:
@@ -581,6 +629,10 @@ 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

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

@@ -35,14 +35,14 @@ ARM_STOP_MSG = ArmManual()  # "
 BIO_STOP_MSG = BioControl()  # "
 
 control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
+    # deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5),
+    # liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"