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.

src/anchor_pkg/launch/rover.launch.py

@@ -1,8 +1,10 @@
 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():
@@ -46,50 +48,24 @@ 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="arm_pkg",
-            executable="arm",
-            name="arm",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
-    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",
-            executable="bio",
-            name="bio",
-            output="both",
-            parameters=[{"launch_mode": "anchor"}],
-            on_exit=Shutdown(),
-        )
-    )
-
     ld.add_action(
         Node(
             package="anchor_pkg",
@@ -108,4 +84,91 @@ def generate_launch_description():
         )
     )
 
+    ld.add_action(
+        Node(
+            package="arm_pkg",
+            executable="arm",
+            name="arm",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="bio_pkg",
+            executable="bio",
+            name="bio",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="core",
+            name="core",
+            output="both",
+            parameters=[
+                {"launch_mode": "anchor"},
+                {
+                    "use_ros2_control": LaunchConfiguration(
+                        "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>
-  <depend>python3-can</depend>
+
+  <exec_depend>common_interfaces</exec_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",
-    license="All Rights Reserved",
+    description="ASTRA VicCAN driver package, using python-can and pyserial.",
+    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>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Relays topics related to Arm between VicCAN (through Anchor) and 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-numpy</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_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>
-  <description>TODO: Package description</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <version>1.0.0</version>
+  <description>Biosensor package to handle command interpretation and embedded interfacing.</description>
+  <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",
-    license="TODO: License declaration",
+    description="Relays topics related to Biosensor between VicCAN (through Anchor) and basestation.",
+    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/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>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Relays topics related to Core between VicCAN (through Anchor) and 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-scipy</depend>
-  <depend>python-crccheck-pip</depend>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_depend>
+  <exec_depend>python3-scipy</exec_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_email="tristanmcginnis26@gmail.com",
-    description="Core rover control package to handle command interpretation and embedded interfacing.",
-    license="All Rights Reserved",
+    maintainer="David Sharpe",
+    maintainer_email="ds0196@uah.edu",
+    description="Relays topics related to Core between VicCAN (through Anchor) and basestation.",
+    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>
-  <depend>astra_msgs</depend>
+
+  <exec_depend>common_interfaces</exec_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.",
-    license="All Rights Reserved",
+    description="Provides headless rover control, similar to Basestation.",
+    license="AGPL-3.0-only",
     entry_points={
-        "console_scripts": [
-            "headless_full = src.headless_node:main",
-        ],
+        "console_scripts": ["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
-            input.angular.z = -1.0 * copysign(
+            twist.linear.x = -1.0 * left_stick_y
+            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)
-            self.get_logger().info(
-                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+            if self.use_cmd_vel:
+                header = Header(stamp=self.get_clock().now().to_msg())
+                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
-            # TODO: linear actuator
+            # X: laser
+            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()),