feat(core): change rover_platform parameter to override, default with hostname instead of environment variable

Launch file uses clucky, testbed, auto. Node was using core, testbed, auto. Replaced core with clucky in the node.

README.md

@@ -60,6 +60,33 @@ $ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected
 $ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
 ```
 
+### Using the Mock Connector
+
+Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
+```
+
+To see all data that would be sent over the CAN network (and thus to the microcontrollers), use this command:
+
+```bash
+$ ros2 topic echo /anchor/to_vic/debug
+```
+
+To send data to the mock connector (as if you were a ROS2 node), use the normal relay topic:
+
+```bash
+$ ros2 topic pub /anchor/to_vic/relay astra_msgs/msg/VicCAN '{mcu_name: "core", command_id: 50, data: [0.0, 2.0, 0.0, 1.0]}'
+
+```
+
+To send data to the mock connector (as if you were a microcontroller), publish to the dedicated topic:
+
+```bash
+$ ros2 topic pub /anchor/from_vic/mock_mcu astra_msgs/msg/VicCAN '{mcu_name: "arm", command_id: 55, data: [0.0, 450.0, 900.0, 0.0]}'
+```
+
 ### Testing Serial
 
 You can fake the presence of a Serial device (i.e., MCU) by using the following command:
@@ -68,10 +95,31 @@ 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` environment variable to point it to the fake serial port, like so:
+When you go to run anchor, use the `serial_override` ROS2 parameter to point it to the fake serial port, like so:
 
 ```bash
-$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
+```
+
+### Testing CAN
+
+You can create a virtual CAN network by using the following commands to create and then enable it:
+
+```bash
+sudo ip link add dev vcan0 type vcan
+sudo ip link set vcan0 up
+```
+
+When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
+```
+
+Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
+
+```bash
+$ sudo ip link delete vcan0
 ```
 
 ### Connecting the GuliKit Controller

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,9 +1,9 @@
+from warnings import deprecated
 import rclpy
 from rclpy.node import Node
 from rclpy.executors import ExternalShutdownException
 from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
-import signal
 import atexit
 
 from .connector import (
@@ -15,11 +15,10 @@ from .connector import (
     NoWorkingDeviceException,
 )
 from .convert import string_to_viccan
-import sys
 import threading
 
-from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
+from std_msgs.msg import String
 
 
 class Anchor(Node):
@@ -33,12 +32,19 @@ class Anchor(Node):
         - VicCAN messages for Arm node
     * /anchor/from_vic/bio
         - VicCAN messages for Bio node
+    * /anchor/to_vic/debug
+        - A string copy of the messages published to ./relay are published here
 
     Subscribers:
     * /anchor/from_vic/mock_mcu
-        - For testing without an actual MCU, publish strings here as if they came from an MCU
+        - For testing without an actual MCU, publish ViCAN messages here as if they came from an MCU
     * /anchor/to_vic/relay
         - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+    * /anchor/to_vic/relay_string
+        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
+    * /anchor/relay
+        - Legacy method for talking to connectors. Takes String as input, but does not send the raw strings to connectors.
+          Instead, it converts them to VicCAN messages first.
     """
 
     connector: Connector
@@ -48,6 +54,8 @@ class Anchor(Node):
 
         logger = self.get_logger()
 
+        # ROS2 Parameter Setup
+
         self.declare_parameter(
             "connector",
             "auto",
@@ -59,78 +67,163 @@ class Anchor(Node):
             ),
         )
 
+        self.declare_parameter(
+            "can_override",
+            "",
+            ParameterDescriptor(
+                name="can_override",
+                description="Overrides which CAN channel will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
+            ),
+        )
+
+        self.declare_parameter(
+            "serial_override",
+            "",
+            ParameterDescriptor(
+                name="serial_override",
+                description="Overrides which serial port will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
+            ),
+        )
+
         # Determine which connector to use. Options are Mock, Serial, and CAN
         connector_select = (
             self.get_parameter("connector").get_parameter_value().string_value
         )
-
+        can_override = (
+            self.get_parameter("can_override").get_parameter_value().string_value
+        )
+        serial_override = (
+            self.get_parameter("serial_override").get_parameter_value().string_value
+        )
         match connector_select:
             case "serial":
                 logger.info("using serial connector")
-                self.connector = SerialConnector(self.get_logger())
+                self.connector = SerialConnector(
+                    logger, self.get_clock(), serial_override
+                )
             case "can":
                 logger.info("using CAN connector")
-                self.connector = CANConnector(self.get_logger())
+                self.connector = CANConnector(logger, self.get_clock(), can_override)
             case "mock":
                 logger.info("using mock connector")
-                self.connector = MockConnector(self.get_logger())
+                self.connector = MockConnector(logger, self.get_clock())
             case "auto":
                 logger.info("automatically determining connector")
                 try:
                     logger.info("trying CAN connector")
-                    self.connector = CANConnector(self.get_logger())
+                    self.connector = CANConnector(
+                        logger, self.get_clock(), can_override
+                    )
                 except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
                     logger.info("CAN connector failed, trying serial connector")
-                    self.connector = SerialConnector(self.get_logger())
+                    self.connector = SerialConnector(
+                        logger, self.get_clock(), serial_override
+                    )
             case _:
-                self.get_logger().fatal(
+                logger.fatal(
                     f"invalid value for connector parameter: {connector_select}"
                 )
                 exit(1)
 
-        # Close devices on exit
-        atexit.register(self.cleanup)
-
         # ROS2 Topic Setup
 
         # Publishers
-        self.fromvic_debug_pub_ = self.create_publisher(
+        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
-            String, "/anchor/from_vic/debug", 20
+            String,
+            "/anchor/from_vic/debug",
+            20,
         )
         self.fromvic_core_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/core", 20
+            VicCAN,
+            "/anchor/from_vic/core",
+            20,
         )
         self.fromvic_arm_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/arm", 20
+            VicCAN,
+            "/anchor/from_vic/arm",
+            20,
         )
         self.fromvic_bio_pub_ = self.create_publisher(
-            VicCAN, "/anchor/from_vic/bio", 20
+            VicCAN,
+            "/anchor/from_vic/bio",
+            20,
+        )
+        # Debug publisher
+        self.tovic_debug_pub_ = self.create_publisher(
+            VicCAN,
+            "/anchor/to_vic/debug",
+            20,
         )
 
         # Subscribers
         self.tovic_sub_ = self.create_subscription(
-            VicCAN, "/anchor/to_vic/relay", self.connector.write, 20
+            VicCAN,
+            "/anchor/to_vic/relay",
+            self.write_connector,
+            20,
+        )
+        self.tovic_sub_legacy_ = self.create_subscription(
+            String,
+            "/anchor/relay",
+            self.write_connector_legacy,
+            20,
         )
         self.mock_mcu_sub_ = self.create_subscription(
-            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
+            String,
+            "/anchor/from_vic/mock_mcu",
+            self.relay_fromvic,
+            20,
         )
+        self.tovic_string_sub_ = self.create_subscription(
+            String,
+            "/anchor/to_vic/relay_string",
+            self.connector.write_raw,
+            20,
+        )
+
+        # Close devices on exit
+        atexit.register(self.cleanup)
 
     def cleanup(self):
         self.connector.cleanup()
 
-    def read_MCU(self):
+    def read_connector(self):
-        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
+        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
-        output = self.connector.read()
+        viccan, raw = self.connector.read()
 
-        if not output:
+        if raw:
-            return
+            self.fromvic_debug_pub_.publish(String(data=raw))
 
-        self.relay_fromvic(output)
+        if viccan:
+            self.relay_fromvic(viccan)
+
+    def write_connector(self, msg: VicCAN):
+        """Write to the connector and send a copy to /anchor/to_vic/debug"""
+        self.connector.write(msg)
+        self.tovic_debug_pub_.publish(msg)
+
+    @deprecated(
+        "Use /anchor/to_vic/relay or /anchor/to_vic/relay_string instead of /anchor/relay"
+    )
+    def write_connector_legacy(self, msg: String):
+        """Write to the connector by first attempting to convert String to VicCAN"""
+        # please do not reference this code. ~riley
+        for cmd in msg.data.split("\n"):
+            viccan = string_to_viccan(
+                cmd,
+                "anchor",
+                self.get_logger(),
+                self.get_clock().now().to_msg(),
+            )
+            if viccan:
+                self.write_connector(viccan)
 
     def relay_fromvic(self, msg: VicCAN):
-        """Relay a string message from the MCU to the appropriate VicCAN topic"""
+        """Relay a message from the MCU to the appropriate VicCAN topic"""
-        msg.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
-
         if msg.mcu_name == "core":
             self.fromvic_core_pub_.publish(msg)
         elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
@@ -138,15 +231,6 @@ class Anchor(Node):
         elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
             self.fromvic_bio_pub_.publish(msg)
 
-    def on_mock_fromvic(self, msg: String):
-        viccan = string_to_viccan(
-            msg.data,
-            "mock",
-            self.get_logger(),
-        )
-        if viccan:
-            self.relay_fromvic(viccan)
-
 
 def main(args=None):
     try:
@@ -158,16 +242,9 @@ def main(args=None):
 
         rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
         while rclpy.ok():
-            anchor_node.read_MCU()  # Check the MCU for updates
+            anchor_node.read_connector()  # Check the connector for updates
             rate.sleep()
     except (KeyboardInterrupt, ExternalShutdownException):
         print("Caught shutdown signal, shutting down...")
     finally:
         rclpy.try_shutdown()
-
-
-if __name__ == "__main__":
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
-    main()

src/anchor_pkg/anchor_pkg/connector.py

@@ -1,7 +1,8 @@
 from abc import ABC, abstractmethod
 from astra_msgs.msg import VicCAN
+from rclpy.clock import Clock
 from rclpy.impl.rcutils_logger import RcutilsLogger
-from .convert import string_to_viccan
+from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
 
 # CAN
 import can
@@ -20,6 +21,16 @@ KNOWN_USBS = [
 ]
 BAUD_RATE = 115200
 
+MCU_IDS = [
+    "broadcast",
+    "core",
+    "arm",
+    "digit",
+    "faerie",
+    "citadel",
+    "libs",
+]
+
 
 class NoValidDeviceException(Exception):
     pass
@@ -39,18 +50,36 @@ class DeviceClosedException(Exception):
 
 class Connector(ABC):
     logger: RcutilsLogger
+    clock: Clock
 
-    def __init__(self, logger: RcutilsLogger):
+    def string_to_viccan(self, msg: str, mcu_name: str):
+        """function currying so that we do not need to pass logger and clock every time"""
+        return _string_to_viccan(
+            msg,
+            mcu_name,
+            self.logger,
+            self.clock.now().to_msg(),
+        )
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
         self.logger = logger
+        self.clock = clock
 
     @abstractmethod
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        """
+        Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
+        """
         pass
 
     @abstractmethod
     def write(self, msg: VicCAN):
         pass
 
+    @abstractmethod
+    def write_raw(self, msg: str):
+        pass
+
     def cleanup(self):
         pass
 
@@ -59,12 +88,19 @@ class SerialConnector(Connector):
     port: str
     mcu_name: str
     serial_interface: serial.Serial
-    override: bool
 
-    def __init__(self, logger: RcutilsLogger):
+    def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
-        super().__init__(logger)
+        super().__init__(logger, clock)
 
         ports = self._find_ports()
+        mcu_name: str | None = None
+
+        if serial_override:
+            logger.warn(
+                f"using serial_override: `{serial_override}`! this will bypass several checks."
+            )
+            ports = [serial_override]
+            mcu_name = "override"
 
         if len(ports) <= 0:
             raise NoValidDeviceException("no valid serial device found")
@@ -75,7 +111,8 @@ class SerialConnector(Connector):
 
         # check each of our ports to make sure one of them is responding
         port = ports[0]
-        mcu_name = self._get_name(port)
+        # we might already have a name by now if we overrode earlier
+        mcu_name = mcu_name or self._get_name(port)
         if not mcu_name:
             raise NoWorkingDeviceException(
                 f"found {port}, but it did not respond with its name"
@@ -136,34 +173,36 @@ class SerialConnector(Connector):
                     )
 
             if serial_interface.is_open:
+                # turn relay mode off if it failed to respond with its name
+                serial_interface.write(b"can_relay_mode,off\n")
                 serial_interface.close()
         except serial.SerialException as e:
             self.logger.error(f"SerialException when asking for MCU name: {e}")
 
         return None
 
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
         try:
             raw = str(self.serial_interface.readline(), "utf8")
 
             if not raw:
-                return None
+                return (None, None)
 
-            return string_to_viccan(raw, self.mcu_name, self.logger)
+            return (
+                self.string_to_viccan(raw, self.mcu_name),
+                raw,
+            )
         except serial.SerialException as e:
             self.logger.error(f"SerialException: {e}")
             raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
-        except TypeError as e:
-            self.logger.error(f"TypeError: {e}")
-            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
         except Exception:
-            pass  # pretty much no other error matters
+            return (None, None)  # pretty much no other error matters
 
     def write(self, msg: VicCAN):
-        # go from [ w, x, y, z ] -> "w,x,y,z" & round to 7 digits max
+        self.write_raw(viccan_to_string(msg))
-        data = ",".join([str(round(x, 7)) for x in msg.data])
+
-        output = f"can_relay_tovic,{msg.mcu_name},{msg.command_id},{data}\n"
+    def write_raw(self, msg: str):
-        self.serial_interface.write(bytes(output, "utf8"))
+        self.serial_interface.write(bytes(msg, "utf8"))
 
     def cleanup(self):
         self.logger.info(f"closing serial port if open {self.port}")
@@ -175,26 +214,36 @@ class SerialConnector(Connector):
 
 
 class CANConnector(Connector):
-    def __init__(self, logger: RcutilsLogger):
+    def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str):
-        super().__init__(logger)
+        super().__init__(logger, clock)
 
         self.can_channel: str | None = None
         self.can_bus: can.BusABC | None = None
 
-        if self.can_channel is None:
+        avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
-            avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
 
-            if len(avail) == 0:
+        if len(avail) == 0:
-                raise NoValidDeviceException("no CAN interfaces found")
+            raise NoValidDeviceException("no CAN interfaces found")
-            if (l := len(avail)) > 1:
+
-                channels = ", ".join(str(b.get("channel")) for b in avail)
+        # filter to busses whose channel matches the can_override
-                raise MultipleValidDevicesException(
+        if can_override:
-                    f"too many ({l}) CAN interfaces found: [{channels}]"
+            self.logger.info(f"overrode can interface with {can_override}")
+            avail = list(
+                filter(
+                    lambda b: b.get("channel") == can_override,
+                    avail,
                 )
+            )
 
-            bus = avail[0]
+        if (l := len(avail)) > 1:
-            self.can_channel = str(bus.get("channel"))
+            channels = ", ".join(str(b.get("channel")) for b in avail)
-            self.logger.info(f"found CAN interface '{self.can_channel}'")
+            raise MultipleValidDevicesException(
+                f"too many ({l}) CAN interfaces found: [{channels}]"
+            )
+
+        bus = avail[0]
+        self.can_channel = str(bus.get("channel"))
+        self.logger.info(f"found CAN interface '{self.can_channel}'")
 
         try:
             self.can_bus = can.Bus(
@@ -208,9 +257,9 @@ class CANConnector(Connector):
             )
 
         if self.can_channel and self.can_channel.startswith("v"):
-            self.logger.warn("using virtual CAN interface; this is likely vcan*")
+            self.logger.warn("CAN interface is likely virtual")
 
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
         if not self.can_bus:
             raise DeviceClosedException("CAN bus not initialized")
 
@@ -221,7 +270,7 @@ class CANConnector(Connector):
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
         if message is None:
-            return None
+            return (None, None)
 
         arbitration_id = message.arbitration_id & 0x7FF
         data_bytes = bytes(message.data)
@@ -230,21 +279,13 @@ class CANConnector(Connector):
         data_type_key = (arbitration_id >> 6) & 0b11
         command = arbitration_id & 0x3F
 
-        key_to_mcu: dict[int, str] = {
+        try:
-            1: "broadcast",
+            mcu_name = MCU_IDS[mcu_key]
-            2: "core",
+        except IndexError:
-            3: "arm",
-            4: "digit",
-            5: "faerie",
-            6: "citadel",
-        }
-
-        mcu_name = key_to_mcu.get(mcu_key)
-        if mcu_name is None:
             self.logger.warn(
                 f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
             )
-            return None
+            return (None, None)
 
         data: list[float] = []
 
@@ -256,33 +297,33 @@ class CANConnector(Connector):
                     self.logger.warn(
                         f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
+                    return (None, None)
-                (value,) = struct.unpack("<d", data_bytes[:8])
+                (value,) = struct.unpack(">d", data_bytes[:8])
                 data = [float(value)]
             elif data_type_key == 1:
                 if len(data_bytes) < 8:
                     self.logger.warn(
                         f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
+                    return (None, None)
-                v1, v2 = struct.unpack("<ff", data_bytes[:8])
+                v1, v2 = struct.unpack(">ff", data_bytes[:8])
                 data = [float(v1), float(v2)]
             elif data_type_key == 2:
                 if len(data_bytes) < 8:
                     self.logger.warn(
                         f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
                     )
-                    return None
+                    return (None, None)
-                i1, i2, i3, i4 = struct.unpack("<hhhh", data_bytes[:8])
+                i1, i2, i3, i4 = struct.unpack(">hhhh", data_bytes[:8])
                 data = [float(i1), float(i2), float(i3), float(i4)]
             else:
                 self.logger.warn(
                     f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
                 )
-                return None
+                return (None, None)
         except struct.error as e:
             self.logger.error(f"error unpacking CAN payload: {e}")
-            return None
+            return (None, None)
 
         viccan = VicCAN(
             mcu_name=mcu_name,
@@ -295,98 +336,65 @@ class CANConnector(Connector):
             f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
         )
 
-        return viccan
+        return (
+            viccan,
+            f"{viccan.mcu_name},{viccan.command_id},"
+            + ",".join(map(str, list(viccan.data))),
+        )
 
     def write(self, msg: VicCAN):
         if not self.can_bus:
             raise DeviceClosedException("CAN bus not initialized")
 
-        # Build 11-bit arbitration ID according to the VicCAN scheme:
+        # build 11-bit arbitration ID according to VicCAN spec:
         # bits 10..8: targeted MCU key
         # bits 7..6:  data type key
         # bits 5..0:  command
 
-        # Map MCU name to 3-bit key.
+        # map MCU name to 3-bit key.
-        mcu_name = (msg.mcu_name or "").lower()
+        try:
-        mcu_key_map: dict[str, int] = {
+            mcu_id = MCU_IDS.index((msg.mcu_name or "").lower())
-            "broadcast": 1,
+        except ValueError:
-            "core": 2,
-            "arm": 3,
-            "digit": 4,
-            "faerie": 5,
-            "citadel": 6,
-        }
-
-        if mcu_name not in mcu_key_map:
             self.logger.error(
                 f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
             )
             return
 
-        mcu_key = mcu_key_map[mcu_name] & 0b111
+        # determine data type from length:
-
-        # Infer data type key from payload length according to the table:
         # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
-        data_len = len(msg.data)
+        match data_len := len(msg.data):
-        if data_len == 0:
+            case 0:
-            data_type_key = 3
+                data_type = 3
-        elif data_len == 1:
+                data = bytes()
-            data_type_key = 0
+            case 1:
-        elif data_len == 2:
+                data_type = 0
-            data_type_key = 1
+                data = struct.pack(">d", *msg.data)
-        elif data_len == 4:
+            case 2:
-            data_type_key = 2
+                data_type = 1
-        else:
+                data = struct.pack(">ff", *msg.data)
-            # Fallback: treat any other non-zero length as float32 x2
+            case 3 | 4:  # 3 gets treated as 4
-            self.logger.warn(
+                data_type = 2
-                f"unexpected VicCAN data length {data_len}; encoding as float32 x2 (key=1) and truncating/padding as needed"
+                if data_len == 3:
-            )
+                    msg.data.append(0)
-            data_type_key = 1
+                data = struct.pack(">hhhh", *[int(x) for x in msg.data])
-
+            case _:
-        # Command is limited to 6 bits.
-        command = int(msg.command_id)
-        if command < 0:
-            self.logger.error(f"invalid negative command_id for CAN frame: {command}")
-            return
-        if command > 0x3F:
-            self.logger.warn(
-                f"command_id 0x{command:X} exceeds 6-bit range; truncating to lower 6 bits"
-            )
-            command &= 0x3F
-
-        arbitration_id = (
-            ((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (command & 0x3F)
-        )
-
-        # Map VicCAN.data (floats) to up to 8 CAN data bytes.
-        raw_bytes: list[int] = []
-        for value in msg.data:
-            try:
-                b = int(round(value))
-            except (TypeError, ValueError):
                 self.logger.error(
-                    f"non-numeric VicCAN data value: {value}; dropping message"
+                    f"unexpected VicCAN data length: {data_len}; dropping message"
                 )
                 return
 
-            if b < 0 or b > 255:
+        # command is limited to 6 bits.
-                self.logger.warn(
+        command = int(msg.command_id)
-                    f"VicCAN data value {value} out of byte range; clamping into [0, 255]"
+        if command < 0 or command > 0x3F:
-                )
+            self.logger.error(
-                b = max(0, min(255, b))
+                f"invalid command_id for CAN frame: {command}; dropping message"
-
-            raw_bytes.append(b)
-
-        if len(raw_bytes) > 8:
-            self.logger.warn(
-                f"VicCAN data too long for single CAN frame ({len(raw_bytes)} > 8); truncating"
             )
-            raw_bytes = raw_bytes[:8]
+            return
 
         try:
             can_message = can.Message(
-                arbitration_id=arbitration_id,
+                arbitration_id=(mcu_id << 8) | (data_type << 6) | command,
-                data=raw_bytes,
+                data=data,
                 is_extended_id=False,
             )
         except Exception as e:
@@ -403,6 +411,9 @@ class CANConnector(Connector):
             self.logger.error(f"CAN error while sending: {e}")
             raise DeviceClosedException("CAN bus closed unexpectedly")
 
+    def write_raw(self, msg: str):
+        self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
+
     def cleanup(self):
         try:
             if self.can_bus is not None:
@@ -413,11 +424,15 @@ class CANConnector(Connector):
 
 
 class MockConnector(Connector):
-    def __init__(self, logger: RcutilsLogger):
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
-        super().__init__(logger)
+        super().__init__(logger, clock)
+        # No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
 
-    def read(self) -> VicCAN | None:
+    def read(self) -> tuple[VicCAN | None, str | None]:
-        return None
+        return (None, None)
 
     def write(self, msg: VicCAN):
-        print(msg)
+        pass
+
+    def write_raw(self, msg: str):
+        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -1,30 +1,22 @@
 from astra_msgs.msg import VicCAN
+from std_msgs.msg import Header
+from builtin_interfaces.msg import Time
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 
-def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger):
+def string_to_viccan(
+    msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
+) -> VicCAN | None:
     """
     Converts the serial string VicCAN format to a ROS2 VicCAN message.
     Does not fill out the Header of the message.
-    On a failure, it will log at a debug level why it failed.
+    On a failure, it will log at a debug level why it failed and return None.
-
-    Parameters:
-      * msg: str
-        - The message in serial VicCAN format
-      * mcu_name: str
-        - The name of the MCU (e.g. core, citadel, arm)
-      * logger: RcutilsLogger
-        - A logger retrieved from node.get_logger()
-
-    Returns:
-      * VicCAN | None
-        - The VicCAN message on a success or None on a failure
     """
 
-    parts: list[str] = msg.split(",")
+    parts: list[str] = msg.strip().split(",")
 
     # don't need an extra check because len of .split output is always >= 1
-    if parts[0] != "can_relay_fromvic":
+    if not parts[0].startswith("can_relay_"):
         logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
         return None
     elif len(parts) < 3:
@@ -34,8 +26,40 @@ def string_to_viccan(msg: str, mcu_name: str, logger: RcutilsLogger):
         logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
         return None
 
-    return VicCAN(
+    try:
-        mcu_name=parts[1],
+        command_id = int(parts[2])
-        command_id=int(parts[2]),
+    except ValueError:
-        data=[float(x) for x in parts[3:]],
+        logger.debug(
-    )
+            f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    if command_id not in range(64):
+        logger.debug(
+            f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    try:
+        return VicCAN(
+            header=Header(
+                stamp=time,
+                frame_id="from_vic",
+            ),
+            mcu_name=parts[1],
+            command_id=command_id,
+            data=[float(x) for x in parts[3:]],
+        )
+    except ValueError:
+        logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
+        return None
+
+
+def viccan_to_string(viccan: VicCAN) -> str:
+    """Converts a ROS2 VicCAN message to the serial string VicCAN format."""
+    # make sure we accept 3 digits and treat it as 4
+    if len(viccan.data) == 3:
+        viccan.data.append(0)
+    # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max
+    data = "".join([f",{round(val,7)}" for val in viccan.data])
+    return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n"

src/anchor_pkg/launch/rover.launch.py

@@ -1,12 +1,16 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.actions import DeclareLaunchArgument, Shutdown, IncludeLaunchDescription
 from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 
 def generate_launch_description():
     connector = LaunchConfiguration("connector")
+    serial_override = LaunchConfiguration("serial_override")
+    can_override = LaunchConfiguration("can_override")
     use_ptz = LaunchConfiguration("use_ptz")
 
     ld = LaunchDescription()
@@ -16,7 +20,23 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "connector",
             default_value="auto",
-            description="Connector parameter for anchor node (default: auto)",
+            description="Connector parameter for anchor node (default: 'auto')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "serial_override",
+            default_value="",
+            description="Serial port override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "can_override",
+            default_value="",
+            description="CAN network override parameter for anchor node (default: '')",
         )
     )
 
@@ -28,7 +48,42 @@ def generate_launch_description():
         )
     )
 
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_ros2_control",
+            default_value="false",
+            description="Whether to use DiffDriveController for driving instead of direct Twist",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "rover_platform_override",
+            default_value="",
+            description="Override the rover platform (either clucky or testbed). If unset, hostname is used; defaults to clucky without hostname.",
+            choices=["clucky", "testbed", ""],
+        )
+    )
+
     # nodes
+    ld.add_action(
+        Node(
+            package="anchor_pkg",
+            executable="anchor",
+            name="anchor",
+            output="both",
+            parameters=[
+                {
+                    "launch_mode": "anchor",
+                    "connector": connector,
+                    "serial_override": serial_override,
+                    "can_override": can_override,
+                }
+            ],
+            on_exit=Shutdown(),
+        )
+    )
+
     ld.add_action(
         Node(
             package="arm_pkg",
@@ -40,27 +95,6 @@ def generate_launch_description():
         )
     )
 
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="core",
-            name="core",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    ld.add_action(
-        Node(
-            package="core_pkg",
-            executable="ptz",
-            name="ptz",
-            output="both",
-            condition=IfCondition(use_ptz),
-        )
-    )
-
     ld.add_action(
         Node(
             package="bio_pkg",
@@ -74,18 +108,67 @@ def generate_launch_description():
 
     ld.add_action(
         Node(
-            package="anchor_pkg",
+            package="core_pkg",
-            executable="anchor",
+            executable="core",
-            name="anchor",
+            name="core",
             output="both",
             parameters=[
+                {"launch_mode": "anchor"},
                 {
-                    "launch_mode": "anchor",
+                    "use_ros2_control": LaunchConfiguration(
-                    "connector": connector,
+                        "use_ros2_control", default=False
-                }
+                    )
+                },
+                {
+                    "rover_platform_override": LaunchConfiguration(
+                        "rover_platform_override", default="auto"
+                    )
+                },
             ],
             on_exit=Shutdown(),
         )
     )
 
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="ptz",
+            name="ptz",
+            output="both",
+            condition=IfCondition(use_ptz),
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "robot_state_publisher.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
+    ld.add_action(
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource(
+                PathJoinSubstitution(
+                    [
+                        FindPackageShare("core_description"),
+                        "launch",
+                        "spawn_controllers.launch.py",
+                    ]
+                )
+            ),
+            condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+            launch_arguments={("hardware_mode", "physical")},
+        )
+    )
+
     return ld

src/anchor_pkg/package.xml

@@ -3,14 +3,20 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>Anchor -- ROS and CAN relay node</description>
+  <description>ASTRA VicCAN driver package, using python-can and pyserial.</description>
   <maintainer email="rjm0037@uah.edu">Riley</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-serial</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>python3-can</depend>
+  <exec_depend>python3-serial</exec_depend>
+  <exec_depend>python3-can</exec_depend>
+
+  <exec_depend>core_pkg</exec_depend>
+  <exec_depend>arm_pkg</exec_depend>
+  <exec_depend>bio_pkg</exec_depend>
+  <exec_depend>core_description</exec_depend>
 
   <build_depend>black</build_depend>
 

src/anchor_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/anchor_pkg
 [install]
 install_scripts=$base/lib/anchor_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/anchor_pkg/setup.py

@@ -6,7 +6,7 @@ package_name = "anchor_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -17,8 +17,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN",
+    description="ASTRA VicCAN driver package, using python-can and pyserial.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
     },

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,5 +1,4 @@
 import sys
-import threading
 import signal
 import math
 from warnings import deprecated
@@ -13,7 +12,7 @@ from std_msgs.msg import String, Header
 from sensor_msgs.msg import JointState
 from control_msgs.msg import JointJog
 from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual  # TODO: Old topics
-from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
+from astra_msgs.msg import ArmFeedback, ArmCtrlState, VicCAN, RevMotorState
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -26,8 +25,6 @@ control_qos = qos.QoSProfile(
     # liveliness_lease_duration=Duration(seconds=5),
 )
 
-thread = None
-
 
 class ArmNode(Node):
     """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
@@ -115,10 +112,10 @@ class ArmNode(Node):
 
         # Control
 
-        # Manual: /arm/manual/joint_jog is published by Basestation or Headless
+        # Manual: /arm/control/joint_jog is published by Basestation or Headless
         self.man_jointjog_sub_ = self.create_subscription(
             JointJog,
-            "/arm/manual/joint_jog",
+            "/arm/control/joint_jog",
             self.jointjog_callback,
             qos_profile=control_qos,
         )
@@ -129,6 +126,13 @@ class ArmNode(Node):
             self.joint_command_callback,
             qos_profile=control_qos,
         )
+        # State: /arm/control/state is published by Basestation or Headless
+        self.man_state_sub_ = self.create_subscription(
+            ArmCtrlState,
+            "/arm/control/state",
+            self.man_state_callback,
+            qos_profile=control_qos,
+        )
 
         # Feedback
 
@@ -148,9 +152,14 @@ class ArmNode(Node):
 
         # Combined Socket and Digit feedback
         self.arm_feedback_new = ArmFeedback()
+        self.arm_feedback_new.axis0_motor.id = 1
+        self.arm_feedback_new.axis1_motor.id = 2
+        self.arm_feedback_new.axis2_motor.id = 3
+        self.arm_feedback_new.axis3_motor.id = 4
 
         # IK Arm pose
         self.saved_joint_state = JointState()
+        self.saved_joint_state.header.frame_id = "base_link"
         self.saved_joint_state.name = self.all_joint_names
         # ... initialize with zeros
         self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
@@ -173,10 +182,54 @@ class ArmNode(Node):
         # Deadzone
         velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
 
-        self.send_velocities(velocities, msg.header)
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=39,
+                data=velocities[0:4],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=39,
+                data=velocities[4:6],
+                header=msg.header,
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="digit",
+                command_id=26,
+                data=[velocities[6]],
+                header=msg.header,
+            )
+        )
 
         # TODO: use msg.duration
 
+    def man_state_callback(self, msg: ArmCtrlState):
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=18,
+                data=[1.0 if msg.brake_mode else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
+
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                mcu_name="arm",
+                command_id=34,
+                data=[1.0 if msg.laser else 0.0],
+                header=Header(stamp=self.get_clock().now().to_msg()),
+            )
+        )
+
     def joint_command_callback(self, msg: JointState):
         if len(msg.position) < 7 and len(msg.velocity) < 7:
             self.get_logger().debug("Ignoring malformed /joint_command message.")
@@ -202,12 +255,12 @@ class ArmNode(Node):
 
         # Send Axis 0-3
         self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], header=header)
+            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:4], header=header)
         )
         # Send Wrist yaw and roll
         # TODO: Verify embedded
         self.anchor_tovic_pub_.publish(
-            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
+            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:6], header=header)
         )
         # Send End Effector Gripper
         # TODO: Verify m/s received correctly by embedded
@@ -290,6 +343,8 @@ class ArmNode(Node):
                 )
                 return
 
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
         match msg.command_id:
             case 53:  # REV SPARK MAX feedback
                 motorId = round(msg.data[0])
@@ -376,11 +431,14 @@ class ArmNode(Node):
                 )
                 return
 
+        self.arm_feedback_new.header.stamp = msg.header.stamp
+
         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
+                self.arm_feedback_new.digit_voltage.header.stamp = msg.header.stamp
             case 55:  # Arm joint positions
                 self.saved_joint_state.position[4] = math.radians(
                     msg.data[0]

src/arm_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>arm_pkg</name>
   <version>1.0.0</version>
-  <description>Core arm package which handles ROS2 commnuication.</description>
+  <description>Relays topics related to Arm between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-numpy</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.py

@@ -12,9 +12,9 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="David",
+    maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Relays topics related to the arm between VicCAN (through Anchor) and basestation.",
+    description="Relays topics related to Arm between VicCAN (through Anchor) and basestation.",
     license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["arm = arm_pkg.arm_node:main"],

src/bio_pkg/package.xml

@@ -2,14 +2,16 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>bio_pkg</name>
-  <version>0.0.0</version>
+  <version>1.0.0</version>
-  <description>TODO: Package description</description>
+  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>astra_msgs</depend>
+  <exec_depend>common_interfaces</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/bio_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/bio_pkg
 [install]
 install_scripts=$base/lib/bio_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/bio_pkg/setup.py

@@ -14,8 +14,8 @@ setup(
     zip_safe=True,
     maintainer="tristan",
     maintainer_email="tristanmcginnis26@gmail.com",
-    description="TODO: Package description",
+    description="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
-    license="TODO: License declaration",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": ["bio = bio_pkg.bio_node:main"],
     },

src/core_pkg/core_pkg/core_headless.py

@@ -1,112 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-import pygame
-
-import time
-
-import serial
-import sys
-import threading
-import glob
-import os
-
-import importlib
-from std_msgs.msg import String
-from astra_msgs.msg import CoreControl
-
-
-os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-os.environ["SDL_AUDIODRIVER"] = (
-    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
-)
-
-
-max_speed = 90  # Max speed as a duty cycle percentage (1-100)
-
-
-class Headless(Node):
-    def __init__(self):
-        # Initialize pygame first
-        pygame.init()
-        pygame.joystick.init()
-
-        # Wait for a gamepad to be connected
-        self.gamepad = None
-        print("Waiting for gamepad connection...")
-        while pygame.joystick.get_count() == 0:
-            # Process any pygame events to keep it responsive
-            for event in pygame.event.get():
-                if event.type == pygame.QUIT:
-                    pygame.quit()
-                    sys.exit(0)
-            time.sleep(1.0)  # Check every second
-            print("No gamepad found. Waiting...")
-
-        # Initialize the gamepad
-        self.gamepad = pygame.joystick.Joystick(0)
-        self.gamepad.init()
-        print(f"Gamepad Found: {self.gamepad.get_name()}")
-
-        # Now initialize the ROS2 node
-        super().__init__("core_headless")
-        self.create_timer(0.15, self.send_controls)
-        self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
-        self.lastMsg = (
-            String()
-        )  # Used to ignore sending controls repeatedly when they do not change
-
-    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):
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                sys.exit(0)
-
-        # Check if controller is still connected
-        if pygame.joystick.get_count() == 0:
-            print("Gamepad disconnected. Exiting...")
-            # Send one last zero control message
-            input = CoreControl()
-            input.left_stick = 0
-            input.right_stick = 0
-            input.max_speed = 0
-            self.publisher.publish(input)
-            self.get_logger().info("Final stop command sent. Shutting down.")
-            # Clean up
-            pygame.quit()
-            sys.exit(0)
-
-        input = CoreControl()
-        input.max_speed = max_speed
-        input.right_stick = -1 * round(self.gamepad.get_axis(4), 2)  # right y-axis
-        if self.gamepad.get_axis(5) > 0:
-            input.left_stick = input.right_stick
-        else:
-            input.left_stick = -1 * round(self.gamepad.get_axis(1), 2)  # left y-axis
-
-        output = f"L: {input.left_stick}, R: {input.right_stick}, M: {max_speed}"
-        self.get_logger().info(f"[Ctrl] {output}")
-        self.publisher.publish(input)
-
-
-def main(args=None):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

src/core_pkg/core_pkg/core_node.py

@@ -1,20 +1,17 @@
-import rclpy
-from rclpy.node import Node
-from rclpy import qos
-from rclpy.duration import Duration
-from std_srvs.srv import Empty
-
-import signal
-import time
-import atexit
-
-import serial
-import os
 import sys
-import threading
+import signal
-import glob
+from typing import Literal, cast
 from scipy.spatial.transform import Rotation
 from math import copysign, pi
+from warnings import deprecated
+from os import getenv
+from socket import gethostname
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
+from rclpy import qos
+from rclpy.duration import Duration
 
 from std_msgs.msg import String, Header
 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
@@ -23,12 +20,14 @@ from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
 from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
 
-serial_pub = None
-thread = None
-
 CORE_WHEELBASE = 0.836  # meters
 CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1
+
+# TODO: update core_description or add testbed_description
+TESTBED_WHEELBASE = 0.368  # meters
+TESTBED_WHEEL_RADIUS = 0.108  # meters
+TESTBED_GEAR_RATIO = 64  # Testbed: 64:1
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -41,207 +40,191 @@ control_qos = qos.QoSProfile(
     # liveliness_lease_duration=Duration(seconds=5),
 )
 
-# Used to verify the length of an incoming VicCAN feedback message
-# Key is VicCAN command_id, value is expected length of data list
-viccan_msg_len_dict = {
-    48: 1,
-    49: 1,
-    50: 2,
-    51: 4,
-    52: 4,
-    53: 4,
-    54: 4,
-    56: 4,  # really 3, but viccan
-    58: 4,  # ditto
-}
 
+class CoreNode(Node):
+    """Relay between Anchor and Basestation/Headless/Moveit2 for Core related topics."""
+
+    # Used to verify the length of an incoming VicCAN feedback message
+    # Key is VicCAN command_id, value is expected length of data list
+    viccan_msg_len_dict = {
+        48: 1,
+        49: 1,
+        50: 2,
+        51: 4,
+        52: 4,
+        53: 4,
+        54: 4,
+        56: 4,  # really 3, but viccan
+        58: 4,  # ditto
+    }
+    rover_platform: Literal["clucky", "testbed"]
 
-class SerialRelay(Node):
     def __init__(self):
-        # Initalize node with name
         super().__init__("core_node")
 
-        # Launch mode -- anchor vs core
+        self.get_logger().info(f"core launch_mode is: anchor")
-        self.declare_parameter("launch_mode", "core")
-        self.launch_mode = self.get_parameter("launch_mode").value
-        self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
 
         ##################################################
-        # Topics
+        # Parameters
+
+        self.declare_parameter("use_ros2_control", False)
+        self.use_ros2_control = (
+            self.get_parameter("use_ros2_control").get_parameter_value().bool_value
+        )
+
+        self.declare_parameter("rover_platform_override", "")
+        rover_platform = (
+            self.get_parameter("rover_platform_override")
+            .get_parameter_value()
+            .string_value
+        )
+        # Verify rover_platform_override value is valid
+        if rover_platform not in ("clucky", "testbed", ""):
+            # Keeping this here, because I want a value error only if the user manually
+            # overrides using a bad value. If we can't determine it automatically
+            # from hostname, then default to clucky.
+            self.get_logger().fatal(
+                "Invalid rover_platform_override parameter value. If set, must be 'clucky' or 'testbed'."
+            )
+        # Attempt to determine platform from hostname, default to clucky on failure
+        rover_platform = rover_platform or gethostname().lower()
+        if rover_platform not in ("clucky", "testbed"):
+            self.get_logger().info(
+                "rover_platform defaulting to clucky, not overridden and could not determine from hostname."
+            )
+            rover_platform = "clucky"
+        self.rover_platform = cast(Literal["clucky", "testbed"], rover_platform)
+
+        if self.rover_platform == "testbed":
+            global TESTBED_WHEELBASE, TESTBED_WHEEL_RADIUS, TESTBED_GEAR_RATIO
+            self.wheelbase = TESTBED_WHEELBASE
+            self.wheel_radius = TESTBED_WHEEL_RADIUS
+            self.gear_ratio = TESTBED_GEAR_RATIO
+        else:  # default in case of unset or invalid environment variable
+            global CORE_WHEELBASE, CORE_WHEEL_RADIUS, CORE_GEAR_RATIO
+            self.wheelbase = CORE_WHEELBASE
+            self.wheel_radius = CORE_WHEEL_RADIUS
+            self.gear_ratio = CORE_GEAR_RATIO
+
+        ##################################################
+        # Old Topics
+
+        self.anchor_sub = self.create_subscription(
+            String, "/anchor/core/feedback", self.anchor_feedback, 10
+        )
+        self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+
+        if not self.use_ros2_control:
+            # /core/control
+            self.control_sub = self.create_subscription(
+                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()
+
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+
+        ##################################################
+        # New Topics
 
         # Anchor
-        if self.launch_mode == "anchor":
-            self.anchor_fromvic_sub_ = self.create_subscription(
-                VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
-            )
-            self.anchor_tovic_pub_ = self.create_publisher(
-                VicCAN, "/anchor/to_vic/relay", 20
-            )
 
-            self.anchor_sub = self.create_subscription(
+        self.anchor_fromvic_sub_ = self.create_subscription(
-                String, "/anchor/core/feedback", self.anchor_feedback, 10
+            VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
-            )
+        )
-            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
+        self.anchor_tovic_pub_ = self.create_publisher(
+            VicCAN, "/anchor/to_vic/relay", 20
+        )
 
         # 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
+            # manual twist -- [-1, 1] rather than real units
-        )
+            # TODO: change topic to '/core/control/twist'
-        # manual flags -- brake mode and max duty cycle
+            self.twist_man_sub_ = self.create_subscription(
-        self.control_state_sub_ = self.create_subscription(
+                Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
-            CoreCtrlState,
+            )
-            "/core/control/state",
+            # manual flags -- brake mode and max duty cycle
-            self.control_state_callback,
+            self.control_state_sub_ = self.create_subscription(
-            qos_profile=control_qos,
+                CoreCtrlState,
-        )
+                "/core/control/state",
-        self.twist_max_duty = (
+                self.control_state_callback,
-            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+                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
 
-        # Consolidated and organized core feedback
+        # Consolidated and organized main core feedback
+        # TODO: change topic to something like '/core/feedback/main'
         self.feedback_new_pub_ = self.create_publisher(
             NewCoreFeedback,
             "/core/feedback_new",
             qos_profile=qos.qos_profile_sensor_data,
         )
+
+        # Joint states for topic-based controller
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # IMU (embedded BNO-055)
+        self.imu_pub_ = self.create_publisher(
+            Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # GPS (embedded u-blox M9N)
+        self.gps_pub_ = self.create_publisher(
+            NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        # Barometer (embedded BMP-388)
+        self.baro_pub_ = self.create_publisher(
+            Barometer, "/core/feedback/baro", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        ###################################################
+        # Timers
+
+        if self.use_ros2_control:
+            self.vel_cmd_timer_ = self.create_timer(0.1, self.vel_cmd_timer_callback)
+
+        ###################################################
+        # Saved state
+
+        # Controls
+        self._last_joint_command_time = self.get_clock().now()
+        self._last_joint_command_msg = JointState()
+
+        # Main Core feedback
         self.feedback_new_state = NewCoreFeedback()
         self.feedback_new_state.fl_motor.id = 1
         self.feedback_new_state.bl_motor.id = 2
         self.feedback_new_state.fr_motor.id = 3
         self.feedback_new_state.br_motor.id = 4
-        self.telemetry_pub_timer = self.create_timer(
+
-            1.0, self.publish_feedback
+        # IMU
-        )  # 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
-        )
-        # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(
-            Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
-        )
         self.imu_state = Imu()
         self.imu_state.header.frame_id = "core_bno055"
-        # GPS (embedded u-blox M9N)
+
-        self.gps_pub_ = self.create_publisher(
+        # GPS
-            NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
-        )
         self.gps_state = NavSatFix()
         self.gps_state.header.frame_id = "core_gps_antenna"
         self.gps_state.status.service = NavSatStatus.SERVICE_GPS
         self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
         self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
-        # Barometer (embedded BMP-388)
+
-        self.baro_pub_ = self.create_publisher(
+        # Barometer
-            Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
-        )
         self.baro_state = Barometer()
         self.baro_state.header.frame_id = "core_bmp388"
 
-        # Old
+    @deprecated("Uses an old message type. Will be removed at some point.")
-
-        # /core/control
-        self.control_sub = self.create_subscription(
-            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()
-        # Debug
-        self.debug_pub = self.create_publisher(String, "/core/debug", 10)
-        self.ping_service = self.create_service(
-            Empty, "/astra/core/ping", self.ping_callback
-        )
-
-        ##################################################
-        # Find microcontroller (Non-anchor only)
-
-        # Core (non-anchor) specific
-        if self.launch_mode == "core":
-            # Loop through all serial devices on the computer to check for the MCU
-            self.port = None
-            ports = SerialRelay.list_serial_ports()
-            for i in range(2):
-                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("\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}!")
-                            self.get_logger().info(f"Enabling Relay Mode")
-                            ser.write(b"can_relay_mode,on\n")
-                            break
-                    except:
-                        pass
-                if self.port is not None:
-                    break
-
-            if self.port is None:
-                self.get_logger().info("Unable to find MCU...")
-                time.sleep(1)
-                sys.exit(1)
-
-            self.ser = serial.Serial(self.port, 115200)
-            atexit.register(self.cleanup)
-        # end __init__()
-
-    def run(self):
-        # This thread makes all the update processes run in the background
-        global thread
-        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
-        thread.start()
-
-        try:
-            while rclpy.ok():
-                if self.launch_mode == "core":
-                    self.read_MCU()  # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
-
-    def read_MCU(self):  # NON-ANCHOR SPECIFIC
-        try:
-            output = str(self.ser.readline(), "utf8")
-
-            if output:
-                # All output over debug temporarily
-                print(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-                return
-        except serial.SerialException as e:
-            print(f"SerialException: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-        except Exception as e:
-            print(f"Exception: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            sys.exit(1)
-
     def scale_duty(self, value: float, max_speed: float):
         leftMin = -1
         leftMax = 1
@@ -258,6 +241,7 @@ class SerialRelay(Node):
         # Convert the 0-1 range into a value in the right range.
         return str(rightMin + (valueScaled * rightSpan))
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_controls(self, msg: CoreControl):
         if msg.turn_to_enable:
             command = (
@@ -283,17 +267,57 @@ class SerialRelay(Node):
 
         # print(f"[Sys] Relaying: {command}")
 
-    def cmd_vel_callback(self, msg: TwistStamped):
+    def joint_command_callback(self, msg: JointState):
-        linear = msg.twist.linear.x
+        # So... topic based control node publishes JointState messages over /joint_commands
-        angular = -msg.twist.angular.z
+        #  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) != (4 if self.rover_platform == "testbed" else 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[-4:] != [  # type: ignore
+            "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
 
-        vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
+        # A 10 Hz timer callback actually sends these commands to Core for rate limiting
-        vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
+        # These come from ros2_control at 50 Hz
+        self._last_joint_command_time = self.get_clock().now()
+        self._last_joint_command_msg = msg
 
-        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
+    def vel_cmd_timer_callback(self):
-        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
+        # Safety timeout for diff_drive_controller commands via topic_based_ros2_control.
+        # It is safe to send stop command here because if self.use_ros2_control,
+        # then this is the only callback that is controlling Core's motors.
+        if self.get_clock().now() - self._last_joint_command_time > Duration(
+            nanoseconds=int(1e8)  # 100ms
+        ):
+            self.send_viccan(20, [0.0, 0.0, 0.0, 0.0])
+            return
 
-        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
+        # This order is verified by the subscription callback
+        (bl_vel, br_vel, fl_vel, fr_vel) = self._last_joint_command_msg.velocity
+
+        # Convert wheel rad/s to motor RPM
+        bl_rpm = radps_to_rpm(bl_vel) * self.gear_ratio
+        br_rpm = radps_to_rpm(br_vel) * self.gear_ratio
+        fl_rpm = radps_to_rpm(fl_vel) * self.gear_ratio
+        fr_rpm = radps_to_rpm(fr_vel) * self.gear_ratio
+
+        self.send_viccan(20, [fl_rpm, bl_rpm, fr_rpm, br_rpm])  # REV Velocity
 
     def twist_man_callback(self, msg: Twist):
         linear = msg.linear.x  # [-1 1] for forward/back from left stick y
@@ -334,15 +358,9 @@ class SerialRelay(Node):
         # Max duty cycle
         self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_cmd(self, msg: str):
-        if self.launch_mode == "anchor":
+        self.anchor_pub.publish(String(data=msg))  # Publish to anchor for relay
-            # self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()  # Convert to std_msg string
-            output.data = msg
-            self.anchor_pub.publish(output)
-        elif self.launch_mode == "core":
-            self.get_logger().info(f"[Core to MCU] {msg}")
-            self.ser.write(bytes(msg, "utf8"))
 
     def send_viccan(self, cmd_id: int, data: list[float]):
         self.anchor_tovic_pub_.publish(
@@ -354,6 +372,7 @@ class SerialRelay(Node):
             )
         )
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def anchor_feedback(self, msg: String):
         output = msg.data
         parts = str(output.strip()).split(",")
@@ -423,14 +442,16 @@ class SerialRelay(Node):
         # skill diff if not
 
         # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_msg_len_dict:
+        if msg.command_id in self.viccan_msg_len_dict:
-            expected_len = viccan_msg_len_dict[msg.command_id]
+            expected_len = self.viccan_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
 
+        self.feedback_new_state.header.stamp = msg.header.stamp
+
         match msg.command_id:
             # GNSS
             case 48:  # GNSS Latitude
@@ -457,6 +478,7 @@ class SerialRelay(Node):
                 self.imu_state.linear_acceleration.x = float(msg.data[0])
                 self.imu_state.linear_acceleration.y = float(msg.data[1])
                 self.imu_state.linear_acceleration.z = float(msg.data[2])
+                self.feedback_new_state.orientation = float(msg.data[3])
                 # Deal with quaternion
                 r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
                 q = r.as_quat()
@@ -501,6 +523,7 @@ class SerialRelay(Node):
                 self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
                 self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
                 self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
+                self.feedback_new_state.board_voltage.header.stamp = msg.header.stamp
             # Baro
             case 56:  # BMP temperature, altitude, pressure
                 self.baro_state.temperature = float(msg.data[0])
@@ -513,14 +536,12 @@ class SerialRelay(Node):
                 motorId = round(float(msg.data[0]))
                 position = float(msg.data[1])
                 velocity = float(msg.data[2])
-                joint_state_msg = (
+                joint_state_msg = JointState()
-                    JointState()
-                )  # TODO: not sure if all motors should be in each message or not
                 joint_state_msg.position = [
-                    position * (2 * pi) / CORE_GEAR_RATIO
+                    position * (2 * pi) / self.gear_ratio
                 ]  # revolutions to radians
                 joint_state_msg.velocity = [
-                    velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
+                    velocity * (2 * pi / 60.0) / self.gear_ratio
                 ]  # RPM to rad/s
 
                 motor: RevMotorState | None = None
@@ -528,52 +549,42 @@ 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
 
+                if motor:
+                    motor.position = position
+                    motor.velocity = velocity
+
+                # make the fucking shit work
+                if self.rover_platform == "clucky":
+                    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 _:
                 return
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def publish_feedback(self):
         # self.get_logger().info(f"[Core] {self.core_feedback}")
         self.feedback_pub.publish(self.core_feedback)
 
-    def ping_callback(self, request, response):
-        return response
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-
-    def cleanup(self):
-        print("Cleaning up before terminating...")
-        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 map_range(
     value: float, in_min: float, in_max: float, out_min: float, out_max: float
@@ -581,19 +592,31 @@ 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 exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
+
+
 def main(args=None):
     rclpy.init(args=args)
-    sys.excepthook = myexcepthook
 
-    global serial_pub
+    # Catch termination signals and exit cleanly
+    signal.signal(signal.SIGTERM, exit_handler)
 
-    serial_pub = SerialRelay()
+    core_node = CoreNode()
-    serial_pub.run()
+
+    try:
+        rclpy.spin(core_node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":
-    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(
-        signal.SIGTERM, lambda signum, frame: sys.exit(0)
-    )  # Catch termination signals and exit cleanly
     main()

src/core_pkg/core_pkg/core_ptz.py

@@ -262,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().info(message_text)
+        self.get_logger().debug(message_text)
 
     def run_async_func(self, coro):
         """Run an async function in the event loop."""

src/core_pkg/package.xml

@@ -3,15 +3,17 @@
 <package format="3">
   <name>core_pkg</name>
   <version>1.0.0</version>
-  <description>Core rover control package to handle command interpretation and embedded interfacing.</description>
+  <description>Relays topics related to Core between VicCAN (through Anchor) and basestation.</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-scipy</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>python-crccheck-pip</depend>
+  <exec_depend>python3-scipy</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python-crccheck-pip</exec_depend>
+
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/core_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/core_pkg
 [install]
 install_scripts=$base/lib/core_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/core_pkg/setup.py

@@ -4,7 +4,7 @@ package_name = "core_pkg"
 
 setup(
     name=package_name,
-    version="0.0.0",
+    version="1.0.0",
     packages=find_packages(exclude=["test"]),
     data_files=[
         ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
@@ -12,14 +12,13 @@ setup(
     ],
     install_requires=["setuptools"],
     zip_safe=True,
-    maintainer="tristan",
+    maintainer="David Sharpe",
-    maintainer_email="tristanmcginnis26@gmail.com",
+    maintainer_email="ds0196@uah.edu",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
+    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
         "console_scripts": [
             "core = core_pkg.core_node:main",
-            "headless = core_pkg.core_headless:main",
             "ptz = core_pkg.core_ptz:main",
         ],
     },

src/headless_pkg/package.xml

@@ -3,14 +3,15 @@
 <package format="3">
   <name>headless_pkg</name>
   <version>1.0.0</version>
-  <description>Headless rover control package to handle command interpretation and embedded interfacing.</description>
+  <description>Provides headless rover control, similar to Basestation.</description>
   <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
-  <depend>common_interfaces</depend>
+
-  <depend>python3-pygame</depend>
+  <exec_depend>common_interfaces</exec_depend>
-  <depend>astra_msgs</depend>
+  <exec_depend>python3-pygame</exec_depend>
+  <exec_depend>astra_msgs</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/headless_pkg/setup.cfg

@@ -2,3 +2,5 @@
 script_dir=$base/lib/headless_pkg
 [install]
 install_scripts=$base/lib/headless_pkg
+[build_scripts]
+executable= /usr/bin/env python3

src/headless_pkg/setup.py

@@ -14,11 +14,9 @@ setup(
     zip_safe=True,
     maintainer="David Sharpe",
     maintainer_email="ds0196@uah.edu",
-    description="Headless rover control package to handle command interpretation and embedded interfacing.",
+    description="Provides headless rover control, similar to Basestation.",
-    license="All Rights Reserved",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
+        "console_scripts": ["headless_full = src.headless_node:main"],
-            "headless_full = src.headless_node:main",
-        ],
     },
 )

src/headless_pkg/src/headless_node.py

@@ -18,7 +18,7 @@ from std_msgs.msg import Header
 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
+from astra_msgs.msg import CoreCtrlState, ArmCtrlState
 
 import warnings
 
@@ -138,6 +138,11 @@ class Headless(Node):
             self.get_parameter("use_old_topics").get_parameter_value().bool_value
         )
 
+        self.declare_parameter("use_cmd_vel", False)
+        self.use_cmd_vel = (
+            self.get_parameter("use_cmd_vel").get_parameter_value().bool_value
+        )
+
         self.declare_parameter("use_bio", False)
         self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
 
@@ -145,7 +150,6 @@ class Headless(Node):
         self.use_arm_ik = (
             self.get_parameter("use_arm_ik").get_parameter_value().bool_value
         )
-
         # NOTE: only applicable if use_old_topics == True
         self.declare_parameter("use_new_arm_manual_scheme", True)
         self.use_new_arm_manual_scheme = (
@@ -155,6 +159,13 @@ class Headless(Node):
         )
 
         # Check parameter validity
+        if self.use_cmd_vel:
+            self.get_logger().info("Using cmd_vel for core control")
+            global CORE_MODE
+            CORE_MODE = "twist"
+        else:
+            self.get_logger().info("Using astra_msgs/CoreControl for core control")
+
         if self.use_arm_ik and self.use_old_topics:
             self.get_logger().fatal("Old topics do not support arm IK control.")
             sys.exit(1)
@@ -166,6 +177,8 @@ class Headless(Node):
         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)
+        self.arm_brake_mode = False
+        self.arm_laser = False
 
         ##################################################
         # Old Topics
@@ -184,12 +197,18 @@ class Headless(Node):
             self.core_twist_pub_ = self.create_publisher(
                 Twist, "/core/twist", qos_profile=control_qos
             )
+            self.core_cmd_vel_pub_ = self.create_publisher(
+                TwistStamped, "/diff_controller/cmd_vel", qos_profile=control_qos
+            )
             self.core_state_pub_ = self.create_publisher(
                 CoreCtrlState, "/core/control/state", qos_profile=control_qos
             )
 
             self.arm_manual_pub_ = self.create_publisher(
-                JointJog, "/arm/manual_new", qos_profile=control_qos
+                JointJog, "/arm/control/joint_jog", qos_profile=control_qos
+            )
+            self.arm_state_pub_ = self.create_publisher(
+                ArmCtrlState, "/arm/control/state", qos_profile=control_qos
             )
 
             self.arm_ik_twist_publisher = self.create_publisher(
@@ -231,13 +250,19 @@ class Headless(Node):
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
+        # Added so you can tell when it starts running after changing the constant logging to debug from info
+        self.get_logger().info("Defaulting to Core mode. Ready.")
+
     def stop_all(self):
         if self.use_old_topics:
             self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(ARM_STOP_MSG)
             self.bio_publisher.publish(BIO_STOP_MSG)
         else:
-            self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
+            if self.use_cmd_vel:
+                self.core_cmd_vel_pub_.publish(self.core_cmd_vel_stop_msg())
+            else:
+                self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
             if self.use_arm_ik:
                 self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
             else:
@@ -332,18 +357,32 @@ class Headless(Node):
             self.core_publisher.publish(input)
 
         else:  # New topics
-            input = Twist()
+            twist = Twist()
 
             # Forward/back and Turn
-            input.linear.x = -1.0 * left_stick_y
+            twist.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(
+            twist.angular.z = -1.0 * copysign(
                 right_stick_x**2, right_stick_x
             )  # Exponent for finer control (curve)
 
+            # This kinda looks dumb being seperate from the following block, but this
+            # maintains the separation between modifying the control message and sending it.
+            if self.use_cmd_vel:
+                # These scaling factors convert raw stick inputs to absolute m/s and rad/s
+                # values that DiffDriveController will convert to motor RPM, rather than
+                # the plain Twist, which just sends the stick values as duty cycle and
+                # sends that scaled to the motors.
+                twist.linear.x *= 1.0
+                twist.angular.z *= 1.5
+
             # Publish
-            self.core_twist_pub_.publish(input)
+            if self.use_cmd_vel:
-            self.get_logger().info(
+                header = Header(stamp=self.get_clock().now().to_msg())
-                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+                self.core_cmd_vel_pub_.publish(TwistStamped(header=header, twist=twist))
+            else:
+                self.core_twist_pub_.publish(twist)
+            self.get_logger().debug(
+                f"[Core Ctrl] Linear: {round(twist.linear.x, 2)}, Angular: {round(twist.angular.z, 2)}"
             )
 
             # Brake mode
@@ -552,13 +591,26 @@ class Headless(Node):
             )
 
             # A: brake
-            # TODO: Brake mode
+            new_brake_mode = button_a
 
-            # Y: linear actuator
+            # X: laser
-            # TODO: linear actuator
+            new_laser = button_x
 
             self.arm_manual_pub_.publish(arm_input)
 
+            # Only publish state if needed
+            if new_brake_mode != self.arm_brake_mode or new_laser != self.arm_laser:
+                self.arm_brake_mode = new_brake_mode
+                self.arm_laser = new_laser
+                state_msg = ArmCtrlState()
+                state_msg.brake_mode = bool(self.arm_brake_mode)
+                state_msg.laser = bool(self.arm_laser)
+
+                self.arm_state_pub_.publish(state_msg)
+                self.get_logger().info(
+                    f"[Arm State] Brake: {self.arm_brake_mode}, Laser: {self.arm_laser}"
+                )
+
         # IK (ONLY NEW)
         # =============
 
@@ -643,6 +695,11 @@ class Headless(Node):
         else:
             pass  # TODO: implement new bio control topics
 
+    def core_cmd_vel_stop_msg(self):
+        return TwistStamped(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
+        )
+
     def arm_manual_stop_msg(self):
         return JointJog(
             header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),