added ros-gz-sim and ros-gz-bridge to packages

added ROS2 and colcon shell completion
removed topic-based-ros2-control package
2026-02-11 17:30:36 +00:00 · 2025-11-19 15:20:25 -06:00 · 2025-11-13 17:46:20 -06:00 · 2025-11-13 17:43:54 -06:00 · 2025-11-07 01:52:08 -06:00 · 2025-11-07 00:52:51 -06:00
68 changed files with 1387 additions and 8004 deletions
--- a/.envrc
+++ b/.envrc
@@ -0,0 +1 @@
+use flake
--- a/.gitignore
+++ b/.gitignore
@@ -10,4 +10,8 @@ log/
 .vscode/

 #Pycache folder
-__pycache__/
+__pycache__/
+
+#Direnv
+.direnv/
+.venv
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +1,6 @@
-[submodule "src/ros2_interfaces_pkg"]
-	path = src/ros2_interfaces_pkg
-	url = git@github.com:SHC-ASTRA/ros2_interfaces_pkg.git
+[submodule "src/astra_description"]
+	path = src/astra_descriptions
+	url = ../astra_descriptions
+[submodule "src/astra_msgs"]
+	path = src/astra_msgs
+	url = git@github.com:SHC-ASTRA/astra_msgs.git
--- a/README.md
+++ b/README.md
@@ -1,89 +1,148 @@
-# rover-ros2
+# ASTRA Rover ROS2 Packages

 [![License: AGPL v3](https://img.shields.io/badge/License-AGPL_v3-blue.svg)](https://www.gnu.org/licenses/agpl-3.0)

-Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
+Includes all main ROS2 packages for the rover. These are centrally located for modular rover operation.

-You will use this package to launch any module-side ROS2 nodes.
-<br>
+You will use these packages to launch all rover-side ROS2 nodes.

-## Software Pre-reqs
+## Table of Contents

-An acting base station computer will need several things:
+- [Software Prerequisites](#software-prerequisites)
+  - [Nix](#nix)
+  - [ROS2 Humble + rosdep](#ros2-humble--rosdep)
+- [Running](#running)
+  - [Testing Serial](#testing-serial)
+  - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
+- [Common Problems/Toubleshooting](#common-problemstroubleshooting)
+- [Packages](#packages)
+- [Maintainers](#maintainers)

-* ROS2 Humble
-    * Follow the standard ROS2 humble install process. Linux recommended.
-        * https://docs.ros.org/en/humble/Installation.html
-* Colcon
-    * `$ sudo apt update`
-    * `$ sudo apt install python3-colcon-common-extensions`
-* Configured Static IP for Ubiquiti bullet (Process varies by OS)
-    * IP Address: 192.168.1.x
-        * This can be just about anything not already in use. I recommend something 30-39
-    * Net Mask: 255.255.255.0
-    * Gateway: 192.168.1.0
+## Software Prerequisites

-## Launching with ANCHOR
+You need either [ROS2 Humble](https://docs.ros.org/en/humble/index.html)
+with [rosdep](https://docs.ros.org/en/humble/Tutorials/Intermediate/Rosdep.html#rosdep-installation)
+or [Nix](https://nixos.org/download/#nix-install-linux) installed. We recommend
+using Nix.

-ANCHOR (Active Node Controller Hub and Operational Relay)
-Allows for launching all nodes on the rover simulataneously. Additionally, all controls will run through the core's NUC and MCU.
-<br>
-1. SSH to core
-    * Core1: `ssh clucky@192.168.1.69`
-    * Core2: `ssh clucky@192.168.1.70`
-    * Password: \<can be found in the rover-Docs repo under documentation>
-2. Navigate to rover-ros2 workspace
-    * `cd rover-ros2`
-3. Source the workspace
-    * `source install/setup.bash`
-4. Launch ANCHOR
-    * `ros2 launch rover_launch.py mode:=anchor`
+### Nix

-## Launching as Standalone
+With Nix, all you have to do is enter the development shell:

-For use when running independent modules through their respective computers (pi/NUC) without ANCHOR.
+```bash
+$ cd path/to/rover-ros2
+$ nix develop
+```

-1. SSH to the the module's computer
-    * Core1: `ssh clucky@192.168.1.69`
-    * Core2: `ssh clucky@192.168.1.70`
-    * Arm: `ssh arm@192.168.1.70`
-    * Bio: \<TBD>
-    * Password: \<can be found in the rover-Docs repo under documentation>
-2. Run the main node (this sends commands to the MCU)
-    * Navigate to the rover-ros2 workspace (location may vary)
-        * `cd rover-ros2`
-    * Source the workspace
-        * `source install/setup.bash`
-    * Start the node
-        * ARM: `ros2 launch rover_launch.py mode:=arm`
-        * CORE: `ros2 launch rover_launch.py mode:=core`
-        * BIO: `ros2 launch rover_launch.py mode:=bio`
+### ROS2 Humble + rosdep

-## Running Headless
+With ROS2 Humble, start by using rosdep to install dependencies:

-Headless control nodes (for ARM and CORE) allow running of the module on-rover without the operator having ROS2 installed on their machine. You will need a laptop to connect to the pi/NUC in order to launch headless but it can be disconnected after the nodes are spun up.
-<br>
-1. SSH to the the module's computer
-    * Core1: `ssh clucky@192.168.1.69`
-    * Core2: `ssh clucky@192.168.1.70`
-    * Arm: `ssh arm@192.168.1.70`
-    * Password: \<can be found in the rover-Docs repo under documentation>
-2. Run the  headless node
-    * You must have ANCHOR or the module's Standalone node running
-    * Open a new terminal (SSH'd to the module)
-    * Navigate to rover-ros2 workspace
-        * `cd rover-ros2`
-    * Source the workspace
-        * `source install/setup.bash`
-    * Run the node (ensure controller is connected and on x-input mode)
-        * CORE: `ros2 run core_pkg headless`
-        * ARM: `ros2 run arm_pkg headless`
+```bash
+  # Setup rosdep
+$ sudo rosdep init  # only run if you haven't already
+$ rosdep update
+  # Install dependencies
+$ cd path/to/rover-ros2
+$ rosdep install --from-paths src -y --ignore-src
+```

-## Connecting the GuliKit Controller
+## Running

-Connecting the GuliKit Controller (Recommended)
+```bash
+$ colcon build
+$ source install/setup.bash
+  # main launch files:
+$ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected to a MCU on the rover.
+$ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
+```

-* Connect controller to pc with USB-C
+### Testing Serial
+
+You can fake the presence of a Serial device (i.e., MCU) by using the following command:
+
+```bash
+$ 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:
+
+```bash
+$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+```
+
+### Connecting the GuliKit Controller
+
+These instructions apply to the black XBox-style GuliKit controller, primarily used for controlling Arm through Basestation.
+
+* Connect the controller to your PC with a USB-C cable
 * Select the "X-Input" control mode (Windows logo) on the controller.
-* Hold the button next to the symbols (windows, android, switch, etc...)
-* You'll need to release the button and press down again to cycle to the next mode
+  * Hold the button next to the symbols (windows, android, switch, etc...)
+  * You'll need to release the button and press down again to cycle to the next mode
+
+## Common Problems/Troubleshooting
+
+**Q**: When I try to launch the nodes, I receive a `package '' not found` error.
+
+A: Make sure you have sourced the workspace in the current shell:
+
+```bash
+$ source install/setup.bash  # or setup.zsh if using ZSH
+```
+
+**Q**: When I try to launch the nodes, I receive several `FileNotFoundError: [Errno 2]` errors.
+
+A: Sometimes the install files get messed up by running `colcon build` in different shells or updating packages. Try running the following commands to clean up your local build files:
+
+```bash
+$ rm -rf build/ install/
+$ colcon build
+```
+
+**Q**: When I run `colcon build` after the above suggestion, I receive several of the following errors:
+
+```bash
+[0.557s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '' in the environment variable AMENT_PREFIX_PATH doesn't exist
+```
+
+A: Don't worry about it. If you had the workspace sourced, ROS2 will complain about the workspace install files not existing anymore after you deleted them. They will be re-created by `colcon build`, after which you can run `source install/setup.bash` to source the new install files.
+
+**Q**: When I try to launch Anchor, I receive the following errors:
+
+```bash
+[anchor-5] [INFO] [1762239452.937881841] [anchor]: Unable to find MCU...
+...
+[ERROR] [anchor-5]: process has died [pid 101820, exit code 1, cmd '.../rover-ros2/install/anchor_pkg/lib/anchor_pkg/anchor --ros-args -r __node:=anchor --params-file /tmp/launch_params_nmv6tpw4'].
+[INFO] [launch]: process[anchor-5] was required: shutting down launched system
+[INFO] [bio-4]: sending signal 'SIGINT' to process[bio-4]
+[INFO] [ptz-3]: sending signal 'SIGINT' to process[ptz-3]
+[INFO] [core-2]: sending signal 'SIGINT' to process[core-2]
+[INFO] [arm-1]: sending signal 'SIGINT' to process[arm-1]
+...
+```
+
+A: To find a microcontroller to talk to, Anchor sends a ping to every Serial port on your computer. If it does not receive a 'pong' in less than one second, then it aborts. There are a few possible fixes:
+
+- Keep trying to run it until it works
+- Run `lsusb` to see if the microcontroller is detected by your computer.
+- Run `ls /dev/tty*0` to see if there is a valid Serial port enumerated for the microcontroller.
+- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`).
+
+## Packages
+
+- [anchor\_pkg](./src/anchor_pkg) - Handles Serial communication between the various other packages here and the microcontroller.
+- [arm\_pkg](./src/arm_pkg) - Relays controls and sensor data for the arm (socket and digit) between anchor and basestation/headless.
+- [astra\_descriptions](./src/astra_descriptions) - Submodule with URDF-related packages.
+- [bio\_pkg](./src/bio_pkg) - Like arm_pkg, but for CITADEL and FAERIE
+- [core\_pkg](./src/core_pkg) - Like arm_pkg, but for Core
+- [headless\_pkg](./src/headless_pkg) - Simple, non-graphical controller node to work in place of basestation when controlling the rover by itself. This is autostarted with anchor to allow for setup-less control of the rover.
+- [latency\_tester](./src/latency_tester) - A temporary node to test comms latency over ROS2, Serial, and CAN.
+- [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
+- [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
+
+## Maintainers
+
+| Name | Email | Discord |
+| ---- | ----- | ------- |
+| David Sharpe | <ds0196@uah.edu> | `@ddavdd` |
+| Riley McLain | <rjm0037@uah.edu> | `@ryleu` |
--- a/auto_start/auto_start_anchor.sh
+++ b/auto_start/auto_start_anchor.sh
@@ -1,4 +1,7 @@
-#!/bin/bash
+#!/usr/bin/env bash
+set -e
+
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
 source /opt/ros/humble/setup.bash

 # Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
+source $SCRIPT_DIR/../install/setup.bash

 # Launch the ROS 2 node with the desired mode
-ros2 launch rover_launch.py mode:=anchor
+ros2 launch anchor_pkg rover.launch.py mode:=anchor
--- a/auto_start/auto_start_core_headless.sh
+++ b/auto_start/auto_start_core_headless.sh
@@ -1,24 +0,0 @@
-#!/bin/bash
-
-# Wait for a network interface to be up (not necessarily online)
-while ! ip link show | grep -q "state UP"; do
-    echo "[INFO] Waiting for active network interface..."
-    sleep 1
-done
-
-echo "[INFO] Network interface is up!"
-
-# Your actual ROS node start command goes here
-echo "[INFO] Starting ROS node..."
-
-# Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
-
-# Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
-
-# Launch the ROS 2 node
-ros2 run core_pkg headless
--- a/auto_start/auto_start_headless_full.sh
+++ b/auto_start/auto_start_headless_full.sh
@@ -1,4 +1,7 @@
-#!/bin/bash
+#!/usr/bin/env bash
+set -e
+
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
 source /opt/ros/humble/setup.bash

 # Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
+source $SCRIPT_DIR/../install/setup.bash

 # Launch the ROS 2 node
 ros2 run headless_pkg headless_full
--- a/auto_start/start_rosbag.sh
+++ b/auto_start/start_rosbag.sh
@@ -1,8 +1,12 @@
-#!/bin/bash
+#!/usr/bin/env bash
+set -e

-ANCHOR_WS="/home/clucky/rover-ros2"
-AUTONOMY_WS="/home/clucky/rover-Autonomy"
-BAG_LOCATION="/home/clucky/bags/autostart"
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
+
+[[ -z "$ANCHOR_WS" ]] && ANCHOR_WS="$SCRIPT_DIR/.."
+[[ -z "$AUTONOMY_WS" ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
+BAG_LOCATION="$HOME/bags/autostart"
+[[ ! -d "$BAG_LOCATION" ]] && mkdir -p "$BAG_LOCATION"

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -15,7 +19,7 @@ echo "[INFO] Network interface is up!"

 source /opt/ros/humble/setup.bash
 source $ANCHOR_WS/install/setup.bash
-[ -f $AUTONOMY_WS/install/setup.bash ] && source $AUTONOMY_WS/install/setup.bash
+[[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash

 cd $BAG_LOCATION

--- a/flake.lock
+++ b/flake.lock
@@ -24,11 +24,11 @@
        "nixpkgs": "nixpkgs"
      },
      "locked": {
-        "lastModified": 1758094726,
-        "narHash": "sha256-agLnClczRtYY+kQFh5dv4wGNhtFNKK7KFOmypDhsWCs=",
+        "lastModified": 1761810010,
+        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
        "owner": "lopsided98",
        "repo": "nix-ros-overlay",
-        "rev": "9d0557032aadb65df065b1972a632572b57234b5",
+        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
        "type": "github"
      },
      "original": {
--- a/flake.nix
+++ b/flake.nix
@@ -1,30 +1,100 @@
 {
+  description = "Development environment for ASTRA Anchor";
+
  inputs = {
    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs";  # IMPORTANT!!!
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
  };
-  outputs = { self, nix-ros-overlay, nixpkgs }:
-    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (system:
+
+  outputs =
+    {
+      self,
+      nix-ros-overlay,
+      nixpkgs,
+    }:
+    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
+      system:
      let
        pkgs = import nixpkgs {
          inherit system;
          overlays = [ nix-ros-overlay.overlays.default ];
+          config = {
+            # Allow the insecure freeimage package
+            permittedInsecurePackages = [
+              "freeimage-3.18.0-unstable-2024-04-18"
+            ];
+          };
        };
-      in {
+      in
+      {
        devShells.default = pkgs.mkShell {
          name = "ASTRA Anchor";
-          packages = [
-            pkgs.colcon
-            pkgs.python312Packages.pyserial
-            pkgs.python312Packages.pygame
-            (with pkgs.rosPackages.humble; buildEnv {
-              paths = [
-                ros-core ros2cli ros2run ros2bag ament-cmake-core python-cmake-module
-              ];
-            })
+          packages = with pkgs; [
+            colcon
+            (python312.withPackages (
+              p: with p; [
+                pyserial
+                pygame
+                scipy
+                crccheck
+                black
+              ]
+            ))
+            (
+              with rosPackages.jazzy;
+              buildEnv {
+                paths = [
+                  ros-core
+                  ros2cli
+                  ros2run
+                  ros2bag
+                  rviz2
+                  xacro
+                  ament-cmake-core
+                  python-cmake-module
+                  diff-drive-controller
+                  parameter-traits
+                  generate-parameter-library
+                  joint-state-publisher-gui
+                  robot-state-publisher
+                  ros2-control
+                  controller-manager
+                  control-msgs
+                  control-toolbox
+                  moveit-core
+                  moveit-common
+                  moveit-msgs
+                  moveit-ros-planning
+                  moveit-ros-planning-interface
+                  moveit-configs-utils
+                  moveit-ros-move-group
+                  moveit-servo
+                  moveit-simple-controller-manager
+                  pilz-industrial-motion-planner
+                  pick-ik
+                  ompl
+                  chomp-motion-planner
+                  joy
+                  ros-gz-sim
+                  ros-gz-bridge
+                  # ros2-controllers nixpkg does not build :(
+                ];
+              }
+            )
          ];
+          shellHook = ''
+            # Display stuff
+            export DISPLAY=''${DISPLAY:-:0}
+            export QT_X11_NO_MITSHM=1
+	    if [[ ! $DIRENV_IN_ENVRC ]]; then
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete ros2)"
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete colcon)"
+            fi
+          '';
        };
-      });
+      }
+    );
+
  nixConfig = {
    extra-substituters = [ "https://ros.cachix.org" ];
    extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];
--- a/rover_launch.py
+++ b/rover_launch.py
@@ -1,132 +0,0 @@
-#!/usr/bin/env python3
-
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-from launch.substitutions import LaunchConfiguration
-from launch_ros.actions import Node
-
-
-
-#Prevent making __pycache__ directories
-from sys import dont_write_bytecode
-dont_write_bytecode = True
-
-def launch_setup(context, *args, **kwargs):
-    # Retrieve the resolved value of the launch argument 'mode'
-    mode = LaunchConfiguration('mode').perform(context)
-    nodes = []
-
-    if mode == 'anchor':
-        # Launch every node and pass "anchor" as the parameter
-        nodes.append(
-            Node(
-                package='arm_pkg',
-                executable='arm',  # change as needed
-                name='arm',
-                output='both',
-                parameters=[{'launch_mode': mode}],
-                on_exit=Shutdown()
-            )
-        )
-        nodes.append(
-            Node(
-                package='core_pkg',
-                executable='core',  # change as needed
-                name='core',
-                output='both',
-                parameters=[{'launch_mode': mode}],
-                on_exit=Shutdown()
-            )
-        )
-        nodes.append(
-            Node(
-                package='core_pkg',
-                executable='ptz',  # change as needed
-                name='ptz',
-                output='both'
-                # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
-                # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
-            )
-        )
-        nodes.append(
-            Node(
-                package='bio_pkg',
-                executable='bio',  # change as needed
-                name='bio',
-                output='both',
-                parameters=[{'launch_mode': mode}],
-                on_exit=Shutdown()
-            )
-        )
-        nodes.append(
-            Node(
-                package='anchor_pkg',
-                executable='anchor',  # change as needed
-                name='anchor',
-                output='both',
-                parameters=[{'launch_mode': mode}],
-                on_exit=Shutdown()
-            )
-        )
-    elif mode in ['arm', 'core', 'bio', 'ptz']:
-        # Only launch the node corresponding to the provided mode.
-        if mode == 'arm':
-            nodes.append(
-                Node(
-                    package='arm_pkg',
-                    executable='arm',
-                    name='arm',
-                    output='both',
-                    parameters=[{'launch_mode': mode}],
-                    on_exit=Shutdown()
-                )
-            )
-        elif mode == 'core':
-            nodes.append(
-                Node(
-                    package='core_pkg',
-                    executable='core',
-                    name='core',
-                    output='both',
-                    parameters=[{'launch_mode': mode}],
-                    on_exit=Shutdown()
-                )
-            )
-        elif mode == 'bio':
-            nodes.append(
-                Node(
-                    package='bio_pkg',
-                    executable='bio',
-                    name='bio',
-                    output='both',
-                    parameters=[{'launch_mode': mode}],
-                    on_exit=Shutdown()
-                )
-            )
-        elif mode == 'ptz':
-            nodes.append(
-                Node(
-                    package='core_pkg',
-                    executable='ptz',
-                    name='ptz',
-                    output='both',
-                    on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched
-                )
-            )
-    else:
-        # If an invalid mode is provided, print an error.
-        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
-
-    return nodes
-
-def generate_launch_description():
-    declare_arg = DeclareLaunchArgument(
-        'mode',
-        default_value='anchor',
-        description='Launch mode: arm, core, bio, anchor, or ptz'
-    )
-
-    return LaunchDescription([
-        declare_arg,
-        OpaqueFunction(function=launch_setup)
-    ])
--- a/src/anchor_pkg/anchor_pkg/anchor_node.py
+++ b/src/anchor_pkg/anchor_pkg/anchor_node.py
@@ -13,7 +13,7 @@ import threading
 import glob

 from std_msgs.msg import String, Header
-from ros2_interfaces_pkg.msg import VicCAN
+from astra_msgs.msg import VicCAN

 serial_pub = None
 thread = None
@@ -38,35 +38,17 @@ Subscribers:
  * /anchor/to_vic/relay_string
    - Publish raw strings to this topic to send directly to the MCU for debugging
 """
+
+
 class SerialRelay(Node):
    def __init__(self):
        # Initalize node with name
-        super().__init__("anchor_node")#previously 'serial_publisher'
-
-        # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
-        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
-        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
-        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
-
-        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
-        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
-        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
-
-
-        # Create publishers 
-        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
-        self.core_pub = self.create_publisher(String, '/anchor/core/feedback', 10)
-        self.bio_pub = self.create_publisher(String, '/anchor/bio/feedback', 10)
-
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
-
-        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
+        super().__init__("anchor_node")  # previously 'serial_publisher'

        # Loop through all serial devices on the computer to check for the MCU
        self.port = None
-        # self.port = "/tmp/ttyACM9"  # Fake port, for debugging
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.port = port_override
        ports = SerialRelay.list_serial_ports()
        for i in range(4):
            if self.port is not None:
@@ -75,7 +57,7 @@ class SerialRelay(Node):
                try:
                    # connect and send a ping command
                    ser = serial.Serial(port, 115200, timeout=1)
-                    #(f"Checking port {port}...")
+                    # (f"Checking port {port}...")
                    ser.write(b"ping\n")
                    response = ser.read_until(bytes("\n", "utf8"))

@@ -97,36 +79,75 @@ class SerialRelay(Node):
        self.ser.write(b"can_relay_mode,on\n")
        atexit.register(self.cleanup)

+        # New pub/sub with VicCAN
+        self.fromvic_debug_pub_ = self.create_publisher(
+            String, "/anchor/from_vic/debug", 20
+        )
+        self.fromvic_core_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/core", 20
+        )
+        self.fromvic_arm_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/arm", 20
+        )
+        self.fromvic_bio_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/bio", 20
+        )
+
+        self.mock_mcu_sub_ = self.create_subscription(
+            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
+        )
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+        )
+        self.tovic_debug_sub_ = self.create_subscription(
+            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+        )
+
+        # Create publishers
+        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
+        self.core_pub = self.create_publisher(String, "/anchor/core/feedback", 10)
+        self.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
+
+        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
+
+        # Create a subscriber
+        self.relay_sub = self.create_subscription(
+            String, "/anchor/relay", self.on_relay_tovic_string, 10
+        )

    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():
-                self.read_MCU() # Check the MCU for updates
+                self.read_MCU()  # Check the MCU for updates
        except KeyboardInterrupt:
            sys.exit(0)

    def read_MCU(self):
-        """ Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
+        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
        try:
            output = str(self.ser.readline(), "utf8")
-            
+
            if output:
                self.relay_fromvic(output)
                # All output over debug temporarily
-                #self.get_logger().info(f"[MCU] {output}")
+                # self.get_logger().info(f"[MCU] {output}")
                msg = String()
                msg.data = output
                self.debug_pub.publish(msg)
                if output.startswith("can_relay_fromvic,core"):
                    self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for voltage readings
                    self.arm_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
+                if output.startswith("can_relay_fromvic,citadel") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for SHT sensor
                    self.bio_pub.publish(msg)
                # msg = String()
                # msg.data = output
@@ -151,15 +172,13 @@ class SerialRelay(Node):
            #     self.ser.close()
            # exit(1)

-
    def on_mock_fromvic(self, msg: String):
-        """ For testing without an actual MCU, publish strings here as if they came from an MCU """
+        """For testing without an actual MCU, publish strings here as if they came from an MCU"""
        # self.get_logger().info(f"Got command from mock MCU: {msg}")
        self.relay_fromvic(msg.data)

-
    def on_relay_tovic_viccan(self, msg: VicCAN):
-        """ Relay a VicCAN message to the MCU """
+        """Relay a VicCAN message to the MCU"""
        output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
        for num in msg.data:
            output += f",{round(num, 7)}"  # limit to 7 decimal places
@@ -168,18 +187,52 @@ class SerialRelay(Node):
        self.ser.write(bytes(output, "utf8"))

    def relay_fromvic(self, msg: str):
-        """ Relay a string message from the MCU to the appropriate VicCAN topic """
+        """Relay a string message from the MCU to the appropriate VicCAN topic"""
        self.fromvic_debug_pub_.publish(String(data=msg))
        parts = msg.strip().split(",")
-        if len(parts) < 3 or parts[0] != "can_relay_fromvic":
+        if len(parts) > 0 and parts[0] != "can_relay_fromvic":
+            self.get_logger().debug(f"Ignoring non-VicCAN message: '{msg.strip()}'")
            return

+        # String validation
+        malformed: bool = False
+        malformed_reason: str = ""
+        if len(parts) < 3 or len(parts) > 7:
+            malformed = True
+            malformed_reason = (
+                f"invalid argument count (expected [3,7], got {len(parts)})"
+            )
+        elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
+            malformed = True
+            malformed_reason = f"invalid mcu_name '{parts[1]}'"
+        elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
+            malformed = True
+            malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
+        else:
+            for x in parts[3:]:
+                try:
+                    float(x)
+                except ValueError:
+                    malformed = True
+                    malformed_reason = f"data '{x}' is not a float"
+                    break
+
+        if malformed:
+            self.get_logger().warning(
+                f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
+            )
+            return
+
+        # Have valid VicCAN message
+
        output = VicCAN()
        output.mcu_name = parts[1]
        output.command_id = int(parts[2])
        if len(parts) > 3:
            output.data = [float(x) for x in parts[3:]]
-        output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
+        output.header = Header(
+            stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
+        )

        # self.get_logger().info(f"Relaying from MCU: {output}")
        if output.mcu_name == "core":
@@ -189,11 +242,10 @@ class SerialRelay(Node):
        elif output.mcu_name == "citadel" or output.mcu_name == "digit":
            self.fromvic_bio_pub_.publish(output)

-
    def on_relay_tovic_string(self, msg: String):
-        """ Relay a raw string message to the MCU for debugging """
+        """Relay a raw string message to the MCU for debugging"""
        message = msg.data
-        #self.get_logger().info(f"Sending command to MCU: {msg}")
+        # self.get_logger().info(f"Sending command to MCU: {msg}")
        self.ser.write(bytes(message, "utf8"))

    @staticmethod
@@ -205,6 +257,7 @@ class SerialRelay(Node):
        if self.ser.is_open:
            self.ser.close()

+
 def myexcepthook(type, value, tb):
    print("Uncaught exception:", type, value)
    if serial_pub:
@@ -220,7 +273,10 @@ def main(args=None):
    serial_pub = SerialRelay()
    serial_pub.run()

-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
+
+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()
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.substitutions import (
+    LaunchConfiguration,
+    ThisLaunchFileDir,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+
+
+# Prevent making __pycache__ directories
+from sys import dont_write_bytecode
+
+dont_write_bytecode = True
+
+
+def launch_setup(context, *args, **kwargs):
+    # Retrieve the resolved value of the launch argument 'mode'
+    mode = LaunchConfiguration("mode").perform(context)
+    nodes = []
+
+    if mode == "anchor":
+        # Launch every node and pass "anchor" as the parameter
+
+        nodes.append(
+            Node(
+                package="arm_pkg",
+                executable="arm",  # change as needed
+                name="arm",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="core_pkg",
+                executable="core",  # change as needed
+                name="core",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="core_pkg",
+                executable="ptz",  # change as needed
+                name="ptz",
+                output="both",
+                # Currently don't shutdown all nodes if the PTZ node fails, as it is not critical
+                # on_exit=Shutdown()  # Uncomment if you want to shutdown on PTZ failure
+            )
+        )
+        nodes.append(
+            Node(
+                package="bio_pkg",
+                executable="bio",  # change as needed
+                name="bio",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+        nodes.append(
+            Node(
+                package="anchor_pkg",
+                executable="anchor",  # change as needed
+                name="anchor",
+                output="both",
+                parameters=[{"launch_mode": mode}],
+                on_exit=Shutdown(),
+            )
+        )
+    elif mode in ["arm", "core", "bio", "ptz"]:
+        # Only launch the node corresponding to the provided mode.
+        if mode == "arm":
+            nodes.append(
+                Node(
+                    package="arm_pkg",
+                    executable="arm",
+                    name="arm",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "core":
+            nodes.append(
+                Node(
+                    package="core_pkg",
+                    executable="core",
+                    name="core",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "bio":
+            nodes.append(
+                Node(
+                    package="bio_pkg",
+                    executable="bio",
+                    name="bio",
+                    output="both",
+                    parameters=[{"launch_mode": mode}],
+                    on_exit=Shutdown(),
+                )
+            )
+        elif mode == "ptz":
+            nodes.append(
+                Node(
+                    package="core_pkg",
+                    executable="ptz",
+                    name="ptz",
+                    output="both",
+                    on_exit=Shutdown(),  # on fail, shutdown if this was the only node to be launched
+                )
+            )
+    else:
+        # If an invalid mode is provided, print an error.
+        print("Invalid mode provided. Choose one of: arm, core, bio, anchor, ptz.")
+
+    return nodes
+
+
+def generate_launch_description():
+    declare_arg = DeclareLaunchArgument(
+        "mode",
+        default_value="anchor",
+        description="Launch mode: arm, core, bio, anchor, or ptz",
+    )
+
+    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
--- a/src/anchor_pkg/package.xml
+++ b/src/anchor_pkg/package.xml
@@ -8,6 +8,10 @@
  <license>AGPL-3.0-only</license>

  <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-serial</depend>
+
+  <build_depend>black</build_depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/anchor_pkg/setup.py
+++ b/src/anchor_pkg/setup.py
@@ -1,25 +1,25 @@
 from setuptools import find_packages, setup
+from os import path
+from glob import glob

-package_name = 'anchor_pkg'
+package_name = "anchor_pkg"

 setup(
    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        (path.join("share", package_name), ["package.xml"]),
+        (path.join("share", package_name, "launch"), glob("launch/*")),
    ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
    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',
+    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",
    entry_points={
-        'console_scripts': [
-            "anchor = anchor_pkg.anchor_node:main"
-        ],
+        "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
    },
 )
--- a/src/arm_pkg/arm_pkg/arm_headless.py
+++ b/src/arm_pkg/arm_pkg/arm_headless.py
@@ -12,14 +12,17 @@ import glob
 import os

 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import ControllerState
-from ros2_interfaces_pkg.msg import ArmManual
-from ros2_interfaces_pkg.msg import ArmIK
+from astra_msgs.msg import ControllerState
+from astra_msgs.msg import ArmManual
+from astra_msgs.msg import ArmIK


-os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+os.environ["SDL_AUDIODRIVER"] = (
+    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+)
 os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display

+
 class Headless(Node):
    def __init__(self):
        # Initalize node with name
@@ -30,20 +33,18 @@ class Headless(Node):

        self.create_timer(0.1, self.send_manual)

-
        # Create a publisher to publish any output the pico sends

        # Depricated, kept temporarily for reference
-        #self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10) 
-        self.manual_pub = self.create_publisher(ArmManual, '/arm/control/manual', 10)
+        # self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10)
+        self.manual_pub = self.create_publisher(ArmManual, "/arm/control/manual", 10)

        # Create a subscriber to listen to any commands sent for the pico
-        
+
        # Depricated, kept temporarily for reference
-        #self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)
-
-        #self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
+        # self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)

+        # self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)

        self.laser_status = 0

@@ -66,38 +67,36 @@ class Headless(Node):
        # Initialize the first gamepad, print name to terminal
        self.gamepad = pygame.joystick.Joystick(0)
        self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
+        print(f"Gamepad Found: {self.gamepad.get_name()}")
        #
        #

-
    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():
-                #Check the pico for updates
+                # Check the pico for updates

-                #self.read_feedback()
-                if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
+                # self.read_feedback()
+                if (
+                    pygame.joystick.get_count() == 0
+                ):  # if controller disconnected, wait for it to be reconnected
                    print(f"Gamepad disconnected: {self.gamepad.get_name()}")
-                    
+
                    while pygame.joystick.get_count() == 0:
-                        #self.send_controls() #depricated, kept for reference temporarily
+                        # self.send_controls() #depricated, kept for reference temporarily
                        self.send_manual()
-                        #self.read_feedback()
+                        # self.read_feedback()
                    self.gamepad = pygame.joystick.Joystick(0)
-                    self.gamepad.init() #re-initialized gamepad
+                    self.gamepad.init()  # re-initialized gamepad
                    print(f"Gamepad reconnected: {self.gamepad.get_name()}")
-                    

        except KeyboardInterrupt:
            sys.exit(0)

-
    def send_manual(self):
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
@@ -105,21 +104,20 @@ class Headless(Node):
                exit()
        input = ArmManual()

-
        # Triggers for gripper control
-        if self.gamepad.get_axis(2) > 0:#left trigger
+        if self.gamepad.get_axis(2) > 0:  # left trigger
            input.gripper = -1
-        elif self.gamepad.get_axis(5) > 0:#right trigger
+        elif self.gamepad.get_axis(5) > 0:  # right trigger
            input.gripper = 1

        # Toggle Laser
-        if self.gamepad.get_button(7):#Start
+        if self.gamepad.get_button(7):  # Start
            self.laser_status = 1
-        elif self.gamepad.get_button(6):#Back
+        elif self.gamepad.get_button(6):  # Back
            self.laser_status = 0
        input.laser = self.laser_status

-        if self.gamepad.get_button(5):#right bumper, control effector
+        if self.gamepad.get_button(5):  # right bumper, control effector

            # Left stick X-axis for effector yaw
            if self.gamepad.get_axis(0) > 0:
@@ -133,7 +131,7 @@ class Headless(Node):
            elif self.gamepad.get_axis(3) < 0:
                input.effector_roll = -1

-        else:   # Control arm axis
+        else:  # Control arm axis
            dpad_input = self.gamepad.get_hat(0)
            input.axis0 = 0
            if dpad_input[0] == 1:
@@ -141,44 +139,44 @@ class Headless(Node):
            elif dpad_input[0] == -1:
                input.axis0 = -1

-            if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
+            if self.gamepad.get_axis(0) > 0.15 or self.gamepad.get_axis(0) < -0.15:
                input.axis1 = round(self.gamepad.get_axis(0))

-            if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15:
+            if self.gamepad.get_axis(1) > 0.15 or self.gamepad.get_axis(1) < -0.15:
                input.axis2 = -1 * round(self.gamepad.get_axis(1))

-            if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.15:
+            if self.gamepad.get_axis(4) > 0.15 or self.gamepad.get_axis(4) < -0.15:
                input.axis3 = -1 * round(self.gamepad.get_axis(4))

            # input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis
            # input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis
            # input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis

-
-        #Button Mappings
-        #axis2 -> LT
-        #axis5 -> RT
-        #Buttons0 -> A
-        #Buttons1 -> B
-        #Buttons2 -> X
-        #Buttons3 -> Y
-        #Buttons4 -> LB
-        #Buttons5 -> RB
-        #Buttons6 -> Back
-        #Buttons7 -> Start
+        # Button Mappings
+        # axis2 -> LT
+        # axis5 -> RT
+        # Buttons0 -> A
+        # Buttons1 -> B
+        # Buttons2 -> X
+        # Buttons3 -> Y
+        # Buttons4 -> LB
+        # Buttons5 -> RB
+        # Buttons6 -> Back
+        # Buttons7 -> Start

        input.linear_actuator = 0

-
        if pygame.joystick.get_count() != 0:
-        
-            self.get_logger().info(f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n")
+
+            self.get_logger().info(
+                f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
+            )
            self.manual_pub.publish(input)
        else:
            pass

        pass
-        
+
    # Depricated, kept temporarily for reference
    # def send_controls(self):

@@ -190,7 +188,7 @@ class Headless(Node):

    #     input.lt = self.gamepad.get_axis(2)#left trigger
    #     input.rt = self.gamepad.get_axis(5)#right trigger
-        
+
    #     #input.lb = self.gamepad.get_button(9)#Value must be converted to bool
    #     if(self.gamepad.get_button(4)):#left bumper
    #         input.lb = True
@@ -202,7 +200,7 @@ class Headless(Node):
    #         input.rb = True
    #     else:
    #         input.rb = False
-        
+
    #     #input.plus = self.gamepad.get_button(6)#plus button
    #     if(self.gamepad.get_button(7)):#plus button
    #         input.plus = True
@@ -218,7 +216,7 @@ class Headless(Node):
    #     input.ls_x = round(self.gamepad.get_axis(0),2)#left x-axis
    #     input.ls_y = round(self.gamepad.get_axis(1),2)#left y-axis
    #     input.rs_x = round(self.gamepad.get_axis(3),2)#right x-axis
-    #     input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis  
+    #     input.rs_y = round(self.gamepad.get_axis(4),2)#right y-axis

    #     #input.a = self.gamepad.get_button(1)#A button
    #     if(self.gamepad.get_button(0)):#A button
@@ -241,14 +239,12 @@ class Headless(Node):
    #     else:
    #         input.y = False

-
    #     dpad_input = self.gamepad.get_hat(0)#D-pad input

    #     #not using up/down on DPad
    #     input.d_up = False
    #     input.d_down = False

-
    #     if(dpad_input[0] == 1):#D-pad right
    #         input.d_right = True
    #     else:
@@ -257,10 +253,9 @@ class Headless(Node):
    #         input.d_left = True
    #     else:
    #         input.d_left = False
-       

    #     if pygame.joystick.get_count() != 0:
-        
+
    #         self.get_logger().info(f"[Ctrl] Updated Controller State\n")

    #         self.publisher.publish(input)
@@ -268,20 +263,17 @@ class Headless(Node):
    #         pass


-
-        
-
 def main(args=None):
    rclpy.init(args=args)

    node = Headless()
-    
+
    rclpy.spin(node)
    rclpy.shutdown()

-    #tb_bs = BaseStation()
-    #node.run()
+    # tb_bs = BaseStation()
+    # node.run()


-if __name__ == '__main__':
+if __name__ == "__main__":
    main()
--- a/src/arm_pkg/arm_pkg/arm_node.py
+++ b/src/arm_pkg/arm_pkg/arm_node.py
@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy import qos
 import serial
 import sys
 import threading
@@ -9,23 +8,11 @@ import time
 import atexit
 import signal
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import ArmManual
-from ros2_interfaces_pkg.msg import ArmIK
-from ros2_interfaces_pkg.msg import SocketFeedback
-from ros2_interfaces_pkg.msg import DigitFeedback
-
-
-# IK-Related imports
-import numpy as np
-import time, math, os
-from math import sin, cos, pi
-from ament_index_python.packages import get_package_share_directory
-# from ikpy.chain import Chain
-# from ikpy.link import OriginLink, URDFLink
-# #import pygame as pyg
-# from scipy.spatial.transform import Rotation as R
-
-from . import astra_arm
+from astra_msgs.msg import ArmManual
+from astra_msgs.msg import SocketFeedback
+from astra_msgs.msg import DigitFeedback
+from sensor_msgs.msg import JointState
+import math

 # control_qos = qos.QoSProfile(
 #     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -48,42 +35,64 @@ class SerialRelay(Node):
        super().__init__("arm_node")

        # Get launch mode parameter
-        self.declare_parameter('launch_mode', 'arm')
-        self.launch_mode = self.get_parameter('launch_mode').value
+        self.declare_parameter("launch_mode", "arm")
+        self.launch_mode = self.get_parameter("launch_mode").value
        self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")

        # Create publishers
-        self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
-        self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
-        self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10)
+        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
+        self.socket_pub = self.create_publisher(
+            SocketFeedback, "/arm/feedback/socket", 10
+        )
+        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
        self.feedback_timer = self.create_timer(0.25, self.publish_feedback)

        # Create subscribers
-        self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10)
-        self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
+        self.man_sub = self.create_subscription(
+            ArmManual, "/arm/control/manual", self.send_manual, 10
+        )

-        self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
+        # New messages
+        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
+        self.joint_state = JointState()
+        self.joint_state.name = [
+            "Axis_0_Joint",
+            "Axis_1_Joint",
+            "Axis_2_Joint",
+            "Axis_3_Joint",
+            "Wrist_Differential_Joint",
+            "Wrist-EF_Roll_Joint",
+            "Gripper_Slider_Left",
+        ]
+        self.joint_state.position = [0.0] * len(
+            self.joint_state.name
+        )  # Initialize with zeros
+
+        self.joint_command_sub = self.create_subscription(
+            JointState, "/joint_commands", self.joint_command_callback, 10
+        )

        # Topics used in anchor mode
-        if self.launch_mode == 'anchor':
-            self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+        if self.launch_mode == "anchor":
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/arm/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)

-        self.arm = astra_arm.Arm('arm12.urdf')
        self.arm_feedback = SocketFeedback()
        self.digit_feedback = DigitFeedback()

        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == 'arm':
+        if self.launch_mode == "arm":
            # Loop through all serial devices on the computer to check for the MCU
            self.port = None
            ports = SerialRelay.list_serial_ports()
-            for i in range(4):
+            for _ in range(4):
                for port in ports:
                    try:
                        # connect and send a ping command
                        ser = serial.Serial(port, 115200, timeout=1)
-                        #print(f"Checking port {port}...")
+                        # print(f"Checking port {port}...")
                        ser.write(b"ping\n")
                        response = ser.read_until("\n")  # type: ignore

@@ -96,12 +105,14 @@ class SerialRelay(Node):
                        pass
                if self.port is not None:
                    break
-            
+
            if self.port is None:
-                self.get_logger().info("Unable to find MCU... please make sure it is connected.")
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
+                )
                time.sleep(1)
                sys.exit(1)
-            
+
            self.ser = serial.Serial(self.port, 115200)
            atexit.register(self.cleanup)

@@ -109,12 +120,12 @@ class SerialRelay(Node):
        global thread
        thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
        thread.start()
-        
-        #if in arm mode, will need to read from the MCU 
+
+        # if in arm mode, will need to read from the MCU

        try:
            while rclpy.ok():
-                if self.launch_mode == 'arm':
+                if self.launch_mode == "arm":
                    if self.ser.in_waiting:
                        self.read_mcu()
                    else:
@@ -124,13 +135,12 @@ class SerialRelay(Node):
        finally:
            self.cleanup()

-
-    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
    def read_mcu(self):
        try:
            output = str(self.ser.readline(), "utf8")
            if output:
-                #self.get_logger().info(f"[MCU] {output}")
+                # self.get_logger().info(f"[MCU] {output}")
                msg = String()
                msg.data = output
                self.debug_pub.publish(msg)
@@ -152,79 +162,19 @@ class SerialRelay(Node):
                self.ser.close()
            pass

-    def send_ik(self, msg):
-        # Convert Vector3 to a NumPy array
-        input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z])  # Convert input to a NumPy array
-        # decrease input vector by 90%
-        input_raw = input_raw * 0.2
-
-        if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0:
-            self.get_logger().info("No input, stopping arm.")
-            command = "can_relay_tovic,arm,39,0,0,0,0\n"
-            self.send_cmd(command)
-            return
-
-        # Debug output
-        tempMsg = String()
-        tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
-        #self.debug_pub.publish(tempMsg)
-
-        # Target position is current position + input vector
-        current_position = self.arm.get_position_vector()
-        target_position = current_position + input_raw
-
-
-        #Print for IK DEBUG
-        tempMsg = String()
-        # tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles)
-        tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6]))
-        self.ik_debug.publish(tempMsg)
-        self.get_logger().info(f"[IK] {tempMsg.data}")
-
-        # Debug output for current position
-        #tempMsg.data = "Current Position: " + str(current_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Debug output for target position
-        #tempMsg.data = "Target Position: " + str(target_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Perform IK with the target position
-        if self.arm.perform_ik(target_position, self.get_logger()):
-            # Send command to control
-            
-            #command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n"
-            #self.send_cmd(command)
-            self.get_logger().info(f"IK Success: {target_position}")
-            self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]")
-
-            # tempMsg = String()
-            # tempMsg.data = "IK Success: " + str(target_position)
-            # #self.debug_pub.publish(tempMsg)
-            # tempMsg.data = "Sending: " + str(command)
-            #self.debug_pub.publish(tempMsg)
-
-            # Send the IK angles to the MCU
-            command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n"
-            # self.send_cmd(command)
-
-            # Manual control for Wrist/Effector
-            command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
-            
-            command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-
-            command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
-
-            command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-
-            self.send_cmd(command)
-        else:
-            self.get_logger().info("IK Fail")
-            self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]")
-            # tempMsg = String()
-            # tempMsg.data = "IK Fail"
-            #self.debug_pub.publish(tempMsg)
+    def joint_command_callback(self, msg: JointState):
+        # Embedded takes deg*10, ROS2 uses Radians
+        positions = [math.degrees(pos) * 10 for pos in msg.position]
+        # Axis 2 & 3 URDF direction is inverted
+        positions[2] = -positions[2]
+        positions[3] = -positions[3]

+        # Set target angles for each arm axis for embedded IK PID to handle
+        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
+        # Wrist yaw and roll
+        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
+        # Gripper IK does not have adequate hardware yet
+        self.send_cmd(command)

    def send_manual(self, msg: ArmManual):
        axis0 = msg.axis0
@@ -232,42 +182,42 @@ class SerialRelay(Node):
        axis2 = msg.axis2
        axis3 = msg.axis3

-        #Send controls for arm
-        command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
-        command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
-        
-        #Send controls for end effector
+        # Send controls for arm
+        command = f"can_relay_tovic,arm,18,{int(msg.brake)}\n"
+        command += f"can_relay_tovic,arm,39,{axis0},{axis1},{axis2},{axis3}\n"

-        # command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
-        # command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-        command += "can_relay_tovic,digit,39," + str(msg.effector_yaw) + "," + str(msg.effector_roll) + "\n"
+        # Send controls for end effector

-        command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"

-        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn

-        command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
+        command += f"can_relay_tovic,digit,28,{msg.laser}\n"
+
+        command += f"can_relay_tovic,digit,34,{msg.linear_actuator}\n"

        self.send_cmd(command)

        return
-    
+
    def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
+        if (
+            self.launch_mode == "anchor"
+        ):  # if in anchor mode, send to anchor node to relay
            output = String()
            output.data = msg
            self.anchor_pub.publish(output)
-        elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly
+        elif self.launch_mode == "arm":  # if in standalone mode, send to MCU directly
            self.get_logger().info(f"[Arm to MCU] {msg}")
            self.ser.write(bytes(msg, "utf8"))

    def anchor_feedback(self, msg: String):
        output = msg.data
        if output.startswith("can_relay_fromvic,arm,55"):
-            #pass
+            # pass
            self.updateAngleFeedback(output)
        elif output.startswith("can_relay_fromvic,arm,54"):
-            #pass
+            # pass
            self.updateBusVoltage(output)
        elif output.startswith("can_relay_fromvic,arm,53"):
            self.updateMotorFeedback(output)
@@ -285,6 +235,12 @@ class SerialRelay(Node):
            if len(parts) >= 4:
                self.digit_feedback.wrist_angle = float(parts[3])
                # self.digit_feedback.wrist_roll = float(parts[4])
+                self.joint_state.position[4] = math.radians(
+                    float(parts[4])
+                )  # Wrist roll
+                self.joint_state.position[5] = math.radians(
+                    float(parts[3])
+                )  # Wrist yaw
        else:
            return

@@ -296,34 +252,27 @@ class SerialRelay(Node):
        # Angle feedbacks,
        # split the msg.data by commas
        parts = msg.split(",")
-        
+
        if len(parts) >= 7:
            # Extract the angles from the string
            angles_in = parts[3:7]
            # Convert the angles to floats divide by 10.0
            angles = [float(angle) / 10.0 for angle in angles_in]
-            # angles[0] = 0.0 #override axis0 to zero
-            #
-            #
-            #THIS NEEDS TO BE REMOVED LATER
-            #PLACEHOLDER FOR WRIST VALUE 
-            #
-            #
-            angles.append(0.0)#placeholder for wrist_continuous
-            angles.append(0.0)#placeholder for wrist
-            #
-            #
-            # # Update the arm's current angles
-            self.arm.update_angles(angles)
+
            self.arm_feedback.axis0_angle = angles[0]
            self.arm_feedback.axis1_angle = angles[1]
            self.arm_feedback.axis2_angle = angles[2]
            self.arm_feedback.axis3_angle = angles[3]
-            # self.get_logger().info(f"Angles: {angles}")
-            # #debug publish angles
-            # tempMsg = String()
-            # tempMsg.data = "Angles: " + str(angles)
-            # #self.debug_pub.publish(tempMsg)
+
+            # Joint state publisher for URDF visualization
+            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
+            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
+            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
+            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
+            # Wrist is handled by digit feedback
+            self.joint_state.header.stamp = self.get_clock().now().to_msg()
+            self.joint_state_pub.publish(self.joint_state)
+
        else:
            self.get_logger().info("Invalid angle feedback input format")

@@ -340,7 +289,7 @@ class SerialRelay(Node):
            self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
        else:
            self.get_logger().info("Invalid voltage feedback input format")
-    
+
    def updateMotorFeedback(self, msg: str):
        parts = str(msg.strip()).split(",")
        motorId = round(float(parts[3]))
@@ -364,11 +313,10 @@ class SerialRelay(Node):
            self.arm_feedback.axis0_voltage = voltage
            self.arm_feedback.axis0_current = current

-
    @staticmethod
    def list_serial_ports():
        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        #return glob.glob("/dev/tty[A-Za-z]*")
+        # return glob.glob("/dev/tty[A-Za-z]*")

    def cleanup(self):
        print("Cleaning up...")
@@ -378,11 +326,13 @@ class SerialRelay(Node):
        except Exception as e:
            exit(0)

+
 def myexcepthook(type, value, tb):
    print("Uncaught exception:", type, value)
    if serial_pub:
        serial_pub.cleanup()

+
 def main(args=None):
    rclpy.init(args=args)
    sys.excepthook = myexcepthook
@@ -391,7 +341,10 @@ def main(args=None):
    serial_pub = SerialRelay()
    serial_pub.run()

-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
+
+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()
--- a/src/arm_pkg/arm_pkg/astra_arm.py
+++ b/src/arm_pkg/arm_pkg/astra_arm.py
@@ -1,145 +0,0 @@
-import rclpy
-from rclpy.node import Node
-import numpy as np
-import time, math, os
-from math import sin, cos, pi
-from ament_index_python.packages import get_package_share_directory
-from ikpy.chain import Chain
-from ikpy.link import OriginLink, URDFLink
-#import pygame as pyg
-from scipy.spatial.transform import Rotation as R
-from geometry_msgs.msg import Vector3  
-
-
-# Misc
-degree = pi / 180.0
-
-
-def convert_angles(angles):
-    # Converts angles to the format used for the urdf (contains some dummy joints)
-    return [0.0, math.radians(angles[0]), math.radians(angles[1]), 0.0, math.radians(angles[2]), 0.0, math.radians(angles[3]), 0.0, math.radians(angles[4]), math.radians(angles[5]), 0.0]
-
-
-class Arm:
-    def __init__(self, urdf_name):
-        self.ik_tolerance = 1e-1 #Tolerance (in meters) to determine if solution is valid
-        # URDF file path
-        self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 'urdf', urdf_name)
-        # IKpy Chain        
-        self.chain = Chain.from_urdf_file(self.urdf)
-
-
-        # Arrays for joint states
-        # Some links in the URDF are static (non-joints), these will remain zero for IK
-        # Indexes: Fixed_base, Ax_0, Ax_1, seg1, Ax_2, seg2, ax_3, seg3, continuous, wrist, Effector
-        self.zero_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-        self.current_angles = self.zero_angles
-        self.last_angles = self.zero_angles
-        self.ik_angles = self.zero_angles
-
-        self.current_position: list[float] = []
-        self.target_position = [0.0, 0.0, 0.0] 
-        self.target_orientation: list = [] # Effector orientation desired at target position. 
-                                # Generally orientation for the effector is modified manually by the operator. 
-        
-        # Might not need, copied over from state_publisher.py in ik_test
-        #self.step = 0.03 # Max movement increment
-
-
-    def perform_ik(self, target_position, logger):
-        self.target_position = target_position
-        # Update the target orientation to the current orientation
-        self.update_orientation()
-        # print(f"[IK FOR] Target Position: {self.target_position}")
-        try:
-            # print(f"[TRY] Current Angles: {self.current_angles}")
-            # print(f"[TRY] Target Position: {self.target_position}")
-            # print(f"[TRY] Target Orientation: {self.target_orientation}")
-            self.ik_angles = self.chain.inverse_kinematics(
-                target_position=self.target_position,
-                target_orientation=self.target_orientation,
-                initial_position=self.current_angles,
-                orientation_mode="all"
-            )
-            # Check if the solution is within the tolerance
-            fk_matrix = self.chain.forward_kinematics(self.ik_angles)
-
-            fk_position = fk_matrix[:3, 3]  # type: ignore
-            
-            # print(f"[TRY] FK Position for Solution: {fk_position}")
-
-            error = np.linalg.norm(target_position - fk_position)
-            if error > self.ik_tolerance:
-                logger.info(f"No VALID IK Solution within tolerance. Error: {error}")
-                return False
-            else:
-                logger.info(f"IK Solution Found. Error: {error}")
-                return True
-        except Exception as e:
-            logger.info(f"IK failed for exception: {e}")
-            return False
-    
-    # # Given the FK_Matix for the arm's current pose, update the orientation array
-    # def update_orientation(self, fk_matrix):
-    #     self.target_orientation = fk_matrix[:3, :3]
-    #     return
-    
-    # def update_joints(self, ax_0, ax_1, ax_2, ax_3, wrist):
-    #     self.current_angles = [0.0, 0.0, ax_0, ax_1, ax_2, ax_3, wrist, 0.0]
-    #     return
-
-    # Get current orientation of the end effector and update target_orientation
-    def update_orientation(self):
-
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Update target_orientation to the effector's current orientation
-        self.target_orientation = fk_matrix[:3, :3]  # type: ignore
-
-    # Update current angles to those provided
-    # Resetting last_angles to the new angles
-    #
-    # Use: First call, or when angles are changed manually.
-    def reset_angles(self, angles):
-        # Update angles to the new angles
-        self.current_angles = convert_angles(angles)
-        self.last_angles = self.current_angles
-
-    # Update current angles to those provided
-    # Maintain previous angles in last_angles
-    #
-    # Use: Repeated calls during IK operation
-    def update_angles(self, angles):
-        # Update angles to the new angles
-        self.last_angles = self.current_angles
-        self.current_angles = convert_angles(angles)
-    
-    # Get current X,Y,Z position of end effector
-    def get_position(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix
-        position = fk_matrix[:3, 3]  # type: ignore
-        
-        return position
-
-    # Get current X,Y,Z position of end effector
-    def get_position_vector(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix
-        position = fk_matrix[:3, 3]  # type: ignore
-
-        # Return position as a NumPy array
-        return np.array(position)
-
-    
-    def update_position(self):
-        # FK matrix for arm's current pose
-        fk_matrix = self.chain.forward_kinematics(self.current_angles)
-        
-        # Get the position of the end effector from the FK matrix and update current pos
-        self.current_position = fk_matrix[:3, 3]  # type: ignore
--- a/src/arm_pkg/package.xml
+++ b/src/arm_pkg/package.xml
@@ -8,7 +8,9 @@
  <license>AGPL-3.0-only</license>

  <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-numpy</depend>
+  <depend>astra_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/arm_pkg/setup.cfg
+++ b/src/arm_pkg/setup.cfg
@@ -2,3 +2,5 @@
 script_dir=$base/lib/arm_pkg
 [install]
 install_scripts=$base/lib/arm_pkg
+[build_scripts]
+executable= /usr/bin/env python3
--- a/src/arm_pkg/setup.py
+++ b/src/arm_pkg/setup.py
@@ -2,29 +2,26 @@ from setuptools import find_packages, setup
 import os
 from glob import glob

-package_name = 'arm_pkg'
+package_name = "arm_pkg"

 setup(
    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="1.0.0",
+    packages=find_packages(exclude=["test"]),
    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*')),
-  	    (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
    ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
    zip_safe=True,
-    maintainer='tristan',
-    maintainer_email='tristanmcginnis26@gmail.com',
-    description='TODO: Package description',
-    license='All Rights Reserved',
+    maintainer="tristan",
+    maintainer_email="tristanmcginnis26@gmail.com",
+    description="TODO: Package description",
+    license="All Rights Reserved",
    entry_points={
-        'console_scripts': [
-            'arm = arm_pkg.arm_node:main',
-            'headless = arm_pkg.arm_headless:main'
+        "console_scripts": [
+            "arm = arm_pkg.arm_node:main",
+            "headless = arm_pkg.arm_headless:main",
        ],
    },
 )
--- a/src/arm_pkg/urdf/arm11.urdf
+++ b/src/arm_pkg/urdf/arm11.urdf
@@ -1,239 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!--MX64trntbl + MX106:  ? + 0.153 -->
-  <!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
-  <!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
-  <material name="omni/Blue">
-    <color rgba="0 0 0.8 1"/>
-  </material>
-  <material name="omni/Red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="omni/Green">
-    <color rgba="0 1 0 1"/>
-  </material>
-  <material name="omni/Yellow">
-    <color rgba="1 1 0 1"/>
-  </material>
-  <material name="omni/LightGrey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="omni/DarkGrey">
-    <color rgba="0.4 0.4 0.4 1"/>
-  </material>
-  <!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
-  <!-- The first use of a macro.  This one was defined in youbot_base/base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
-       that equals "base", and uses it to generate names for its component links and joints
-       (e.g., base_link).  The xacro:included origin block is also an argument to the macro.  By convention,
-       the origin block defines where the component is w.r.t its parent (in this case the parent
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <!-- foot for arm-->
-  <link name="base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-    </inertial>
-  </link>
-  <!-- joint between base_link and arm_0_link -->
-  <joint name="arm_joint_0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="arm_link_0"/>
-  </joint>
-  <link name="arm_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-      <material name="omni/LightGrey"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <mass value="0.2"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_1" type="revolute">
-    <parent link="arm_link_0"/>
-    <child link="arm_link_1"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="arm_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <mass value="0.190421352"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_2" type="revolute">
-    <parent link="arm_link_1"/>
-    <child link="arm_link_2"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-      <material name="omni/Red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <mass value="0.29302326"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_3" type="revolute">
-    <parent link="arm_link_2"/>
-    <child link="arm_link_3"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.471"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_3">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-      <material name="omni/Yellow"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <mass value="0.21931466"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_4" type="revolute">
-    <parent link="arm_link_3"/>
-    <child link="arm_link_4"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.377"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_4">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-      <material name="omni/Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_5" type="revolute">
-    <parent link="arm_link_4"/>
-    <child link="arm_link_5"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.132"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <link name="arm_link_5">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_6" type="fixed">
-    <parent link="arm_link_5"/>
-    <child link="arm_link_6"/>
-    <origin rpy="0 0 0" xyz="0 0 0.248"/>
-  </joint>
-  <link name="arm_link_6">
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="1e-12"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
-    </inertial>
-  </link>
-  <!-- END OF ARM LINKS/JOINTS -->
-</robot>
--- a/src/arm_pkg/urdf/arm12.urdf
+++ b/src/arm_pkg/urdf/arm12.urdf
@@ -1,307 +0,0 @@
-<robot name="robot">
-  <link name="base_footprint"></link>
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint" />
-    <child link="base_link" />
-    <origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
-  </joint>
-  <link name="base_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-      <material name="base_link-material">
-        <color rgba="0 0.6038273388475408 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
-    </inertial>
-  </link>
-  <joint name="Axis_0_Joint" type="revolute">
-    <parent link="base_link" />
-    <child link="Axis_0" />
-    <origin xyz="0 0 0.035" rpy="0 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>  </joint>
-  <link name="Axis_0">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-      <material name="Axis_0-material">
-        <color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_Joint" type="revolute">
-    <parent link="Axis_0" />
-    <child link="Axis_1" />
-    <origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>  </joint>
-  <link name="Axis_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-      <material name="Axis_1-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_to_Segment_1" type="fixed">
-    <parent link="Axis_1" />
-    <child link="Segment_1" />
-    <origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-      <material name="Segment_1-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_Joint" type="revolute">
-    <parent link="Segment_1" />
-    <child link="Axis_2" />
-    <origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>  </joint>
-  <link name="Axis_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-      <material name="Axis_2-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_to_Segment_2" type="fixed">
-    <parent link="Axis_2" />
-    <child link="Segment_2" />
-    <origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-      <material name="Segment_2-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_Joint" type="revolute">
-    <parent link="Segment_2" />
-    <child link="Axis_3" />
-    <origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-      <material name="Axis_3-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_to_Segment_3" type="fixed">
-    <parent link="Axis_3" />
-    <child link="Segment_3" />
-    <origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-      <material name="Segment_3-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Wrist_Joint" type="revolute">
-    <parent link="Segment_3" />
-    <child link="Axis_4" />
-    <origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_4">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-      <material name="Axis_4-material">
-        <color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Continuous_Joint" type="revolute">
-    <parent link="Axis_4" />
-    <child link="Axis_4_C" />
-    <origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
-  </joint>
-  <link name="Axis_4_C">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-      <material name="Axis_4_C-material">
-        <color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_4_C_to_Effector" type="fixed">
-    <parent link="Axis_4_C" />
-    <child link="Effector" />
-    <origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
-  </joint>
-  <link name="Effector">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.05 0.01 0.135" />
-      </geometry>
-      <material name="Effector-material">
-        <color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.01 0.01 0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
-    </inertial>
-  </link>
-</robot>
--- a/src/astra_descriptions
+++ b/src/astra_descriptions
--- a/src/astra_msgs
+++ b/src/astra_msgs
--- a/src/bio_pkg/bio_pkg/bio_node.py
+++ b/src/bio_pkg/bio_pkg/bio_node.py
@@ -9,51 +9,56 @@ import time
 import atexit
 import signal
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import BioControl
-from ros2_interfaces_pkg.msg import BioFeedback
+from astra_msgs.msg import BioControl
+from astra_msgs.msg import BioFeedback

 serial_pub = None
 thread = None

+
 class SerialRelay(Node):
    def __init__(self):
        # Initialize node
        super().__init__("bio_node")

        # Get launch mode parameter
-        self.declare_parameter('launch_mode', 'bio')
-        self.launch_mode = self.get_parameter('launch_mode').value
+        self.declare_parameter("launch_mode", "bio")
+        self.launch_mode = self.get_parameter("launch_mode").value
        self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")

        # Create publishers
-        self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10)
-        self.feedback_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10)
-
+        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
+        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)

        # Create subscribers
-        self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10)
-        
+        self.control_sub = self.create_subscription(
+            BioControl, "/bio/control", self.send_control, 10
+        )
+
        # Create a publisher for telemetry
        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)

        # Topics used in anchor mode
-        if self.launch_mode == 'anchor':
-            self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+        if self.launch_mode == "anchor":
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/bio/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)

        self.bio_feedback = BioFeedback()

-
        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == 'bio':
+        if self.launch_mode == "bio":
            # Loop through all serial devices on the computer to check for the MCU
            self.port = None
            for i in range(2):
                try:
                    # connect and send a ping command
-                    set_port = '/dev/ttyACM0' #MCU is controlled through GPIO pins on the PI
+                    set_port = (
+                        "/dev/ttyACM0"  # MCU is controlled through GPIO pins on the PI
+                    )
                    ser = serial.Serial(set_port, 115200, timeout=1)
-                    #print(f"Checking port {port}...")
+                    # print(f"Checking port {port}...")
                    ser.write(b"ping\n")
                    response = ser.read_until("\n")

@@ -64,12 +69,14 @@ class SerialRelay(Node):
                        break
                except:
                    pass
-            
+
            if self.port is None:
-                self.get_logger().info("Unable to find MCU... please make sure it is connected.")
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
+                )
                time.sleep(1)
                sys.exit(1)
-            
+
            self.ser = serial.Serial(self.port, 115200)
            atexit.register(self.cleanup)

@@ -77,12 +84,12 @@ class SerialRelay(Node):
        global thread
        thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
        thread.start()
-        
-        #if in arm mode, will need to read from the MCU 
+
+        # if in arm mode, will need to read from the MCU

        try:
            while rclpy.ok():
-                if self.launch_mode == 'bio':
+                if self.launch_mode == "bio":
                    if self.ser.in_waiting:
                        self.read_mcu()
                    else:
@@ -92,8 +99,7 @@ class SerialRelay(Node):
        finally:
            self.cleanup()

-
-    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
    def read_mcu(self):
        try:
            output = str(self.ser.readline(), "utf8")
@@ -123,69 +129,85 @@ class SerialRelay(Node):
    def send_ik(self, msg):
        pass

-
    def send_control(self, msg: BioControl):
        # CITADEL Control Commands
        ################

-
        # Chem Pumps, only send if not zero
        if msg.pump_id != 0:
-            command = "can_relay_tovic,citadel,27," + str(msg.pump_id) + "," + str(msg.pump_amount) + "\n"
+            command = (
+                "can_relay_tovic,citadel,27,"
+                + str(msg.pump_id)
+                + ","
+                + str(msg.pump_amount)
+                + "\n"
+            )
            self.send_cmd(command)
        # Fans, only send if not zero
        if msg.fan_id != 0:
-            command = "can_relay_tovic,citadel,40," + str(msg.fan_id) + "," + str(msg.fan_duration) + "\n"
+            command = (
+                "can_relay_tovic,citadel,40,"
+                + str(msg.fan_id)
+                + ","
+                + str(msg.fan_duration)
+                + "\n"
+            )
            self.send_cmd(command)
        # Servos, only send if not zero
        if msg.servo_id != 0:
-            command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(int(msg.servo_state)) + "\n"
+            command = (
+                "can_relay_tovic,citadel,25,"
+                + str(msg.servo_id)
+                + ","
+                + str(int(msg.servo_state))
+                + "\n"
+            )
            self.send_cmd(command)
-        

        # LSS (SCYTHE)
        command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
-        #self.send_cmd(command)
-        
+        # self.send_cmd(command)
+
        # Vibration Motor
        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
-        #self.send_cmd(command)
-        
+        # self.send_cmd(command)

-        # FAERIE Control Commands 
+        # FAERIE Control Commands
        ################
-        
+
        # To be reviewed before use#

        # Laser
        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-        #self.send_cmd(command)
+        # self.send_cmd(command)

        # Drill (SCABBARD)
        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
-        #self.send_cmd(command)
+        # self.send_cmd(command)

        # Bio linear actuator
        command += "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n"
        self.send_cmd(command)

-
-
    def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
+        if (
+            self.launch_mode == "anchor"
+        ):  # if in anchor mode, send to anchor node to relay
            output = String()
            output.data = msg
            self.anchor_pub.publish(output)
-        elif self.launch_mode == 'bio': #if in standalone mode, send to MCU directly
+        elif self.launch_mode == "bio":  # if in standalone mode, send to MCU directly
            self.get_logger().info(f"[Bio to MCU] {msg}")
            self.ser.write(bytes(msg, "utf8"))

    def anchor_feedback(self, msg: String):
        output = msg.data
        parts = str(output.strip()).split(",")
-        #self.get_logger().info(f"[Bio Anchor] {msg.data}")
+        # self.get_logger().info(f"[Bio Anchor] {msg.data}")

-        if output.startswith("can_relay_fromvic,citadel,54"):  # bat, 12, 5, Voltage readings * 100
+        if output.startswith(
+            "can_relay_fromvic,citadel,54"
+        ):  # bat, 12, 5, Voltage readings * 100
            self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
            self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
            self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
@@ -199,7 +221,7 @@ class SerialRelay(Node):
    @staticmethod
    def list_serial_ports():
        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        #return glob.glob("/dev/tty[A-Za-z]*")
+        # return glob.glob("/dev/tty[A-Za-z]*")

    def cleanup(self):
        print("Cleaning up...")
@@ -209,11 +231,13 @@ class SerialRelay(Node):
        except Exception as e:
            exit(0)

+
 def myexcepthook(type, value, tb):
    print("Uncaught exception:", type, value)
    if serial_pub:
        serial_pub.cleanup()

+
 def main(args=None):
    rclpy.init(args=args)
    sys.excepthook = myexcepthook
@@ -222,7 +246,10 @@ def main(args=None):
    serial_pub = SerialRelay()
    serial_pub.run()

-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
+
+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()
--- a/src/bio_pkg/package.xml
+++ b/src/bio_pkg/package.xml
@@ -8,7 +8,8 @@
  <license>AGPL-3.0-only</license>

  <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>astra_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/bio_pkg/setup.py
+++ b/src/bio_pkg/setup.py
@@ -1,25 +1,22 @@
 from setuptools import find_packages, setup

-package_name = 'bio_pkg'
+package_name = "bio_pkg"

 setup(
    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
    ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
    zip_safe=True,
-    maintainer='tristan',
-    maintainer_email='tristanmcginnis26@gmail.com',
-    description='TODO: Package description',
-    license='TODO: License declaration',
+    maintainer="tristan",
+    maintainer_email="tristanmcginnis26@gmail.com",
+    description="TODO: Package description",
+    license="TODO: License declaration",
    entry_points={
-        'console_scripts': [
-            'bio = bio_pkg.bio_node:main'
-        ],
+        "console_scripts": ["bio = bio_pkg.bio_node:main"],
    },
 )
--- a/src/core_gazebo/config/ros_gz_bridge.yaml
+++ b/src/core_gazebo/config/ros_gz_bridge.yaml
@@ -1,25 +0,0 @@
-# gz topic published by Sensors plugin
- ros_topic_name: "camera_head/depth/camera_info"
-  gz_topic_name: "camera_head/camera_info"
-  ros_type_name: "sensor_msgs/msg/CameraInfo"
-  gz_type_name: "gz.msgs.CameraInfo"
-  direction: GZ_TO_ROS
-  lazy: true # Determines whether connections are created immediately at startup (when false) or only when data is actually requested by a subscriber (when true), helping to conserve system resources at the cost of potential initial delays in data flow.
-
-# gz topic published by Sensors plugin
- ros_topic_name: "camera_head/depth/color/points"
-  gz_topic_name: "camera_head/points"
-  ros_type_name: "sensor_msgs/msg/PointCloud2"
-  gz_type_name: "gz.msgs.PointCloudPacked"
-  direction: GZ_TO_ROS
-  lazy: true
-  
-# Clock configuration
- ros_topic_name: "clock"
-  gz_topic_name: "clock"
-  ros_type_name: "rosgraph_msgs/msg/Clock"
-  gz_type_name: "gz.msgs.Clock"
-  direction: GZ_TO_ROS
-  lazy: false
-  
-
--- a/src/core_gazebo/launch/core.gazebo.launch.py
+++ b/src/core_gazebo/launch/core.gazebo.launch.py
@@ -1,286 +0,0 @@
-#!/usr/bin/env python3
-
-import os
-from launch import LaunchDescription
-from launch.actions import (
-    AppendEnvironmentVariable,
-    DeclareLaunchArgument,
-    IncludeLaunchDescription
-)
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
-    """
-    Generate a launch description for the Gazebo simulation.
-
-    This function sets up all necessary parameters, paths, and nodes required to launch
-    the Gazebo simulation with a robot. It handles:
-    1. Setting up package paths and constants
-    2. Declaring launch arguments for robot configuration
-    3. Setting up the Gazebo environment
-    4. Spawning the robot in simulation
-
-    Returns:
-        LaunchDescription: A complete launch description for the simulation
-    """
-    # Constants for paths to different files and folders
-    package_name_gazebo = 'core_gazebo'
-    package_name_description = 'core_rover_description'
-    # package_name_moveit = 'mycobot_moveit_config'
-
-    default_robot_name = 'core_rover'
-    gazebo_models_path = 'models'
-    default_world_file = 'pick_and_place_demo.world'
-    gazebo_worlds_path = 'worlds'
-
-    ros_gz_bridge_config_file_path = 'config/ros_gz_bridge.yaml'
-
-    # Set the path to different files and folders
-    pkg_ros_gz_sim = FindPackageShare(package='ros_gz_sim').find('ros_gz_sim')
-    pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)
-    pkg_share_description = FindPackageShare(
-        package=package_name_description).find(package_name_description)
-    # pkg_share_moveit = FindPackageShare(package=package_name_moveit).find(package_name_moveit)
-
-    gazebo_models_path = os.path.join(pkg_share_gazebo, gazebo_models_path)
-    default_ros_gz_bridge_config_file_path = os.path.join(
-        pkg_share_gazebo, ros_gz_bridge_config_file_path)
-    
-    # Get the parent directory of the package share to access all ROS packages
-    ros_packages_path = os.path.dirname(pkg_share_description)
-
-    # Launch configuration variables
-    jsp_gui = LaunchConfiguration('jsp_gui')
-    load_controllers = LaunchConfiguration('load_controllers')
-    robot_name = LaunchConfiguration('robot_name')
-    use_rviz = LaunchConfiguration('use_rviz')
-    use_camera = LaunchConfiguration('use_camera')
-    use_gazebo = LaunchConfiguration('use_gazebo')
-    use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-    world_file = LaunchConfiguration('world_file')
-
-    world_path = PathJoinSubstitution([
-        pkg_share_gazebo,
-        gazebo_worlds_path,
-        world_file
-    ])
-
-    # Set the pose configuration variables
-    x = LaunchConfiguration('x')
-    y = LaunchConfiguration('y')
-    z = LaunchConfiguration('z')
-    roll = LaunchConfiguration('roll')
-    pitch = LaunchConfiguration('pitch')
-    yaw = LaunchConfiguration('yaw')
-
-
-    ################################################################################################
-    # Declare the launch arguments
-
-    declare_robot_name_cmd = DeclareLaunchArgument(
-        name='robot_name',
-        default_value=default_robot_name,
-        description='The name for the robot')
-
-    declare_load_controllers_cmd = DeclareLaunchArgument(
-        name='load_controllers',
-        default_value='true',
-        description='Flag to enable loading of ROS 2 controllers')
-
-    declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-        name='use_robot_state_pub',
-        default_value='true',
-        description='Flag to enable robot state publisher')
-
-    # GUI and visualization arguments
-    declare_jsp_gui_cmd = DeclareLaunchArgument(
-        name='jsp_gui',
-        default_value='false',
-        description='Flag to enable joint_state_publisher_gui')
-
-    declare_use_camera_cmd = DeclareLaunchArgument(
-        name='use_camera',
-        default_value='false',
-        description='Flag to enable the RGBD camera for Gazebo point cloud simulation')
-
-    declare_use_gazebo_cmd = DeclareLaunchArgument(
-        name='use_gazebo',
-        default_value='true',
-        description='Flag to enable Gazebo')
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        name='use_rviz',
-        default_value='true',
-        description='Flag to enable RViz')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        name='use_sim_time',
-        default_value='true',
-        description='Use simulation (Gazebo) clock if true')
-
-    declare_world_cmd = DeclareLaunchArgument(
-        name='world_file',
-        default_value=default_world_file,
-        description='World file name (e.g., simple_demo.world, pick_and_place_demo.world)')
-
-    # Pose arguments
-    declare_x_cmd = DeclareLaunchArgument(
-        name='x',
-        default_value='0.0',
-        description='x component of initial position, meters')
-
-    declare_y_cmd = DeclareLaunchArgument(
-        name='y',
-        default_value='0.0',
-        description='y component of initial position, meters')
-
-    declare_z_cmd = DeclareLaunchArgument(
-        name='z',
-        default_value='0.75',
-        description='z component of initial position, meters')
-
-    declare_roll_cmd = DeclareLaunchArgument(
-        name='roll',
-        default_value='0.0',
-        description='roll angle of initial orientation, radians')
-
-    declare_pitch_cmd = DeclareLaunchArgument(
-        name='pitch',
-        default_value='0.0',
-        description='pitch angle of initial orientation, radians')
-
-    declare_yaw_cmd = DeclareLaunchArgument(
-        name='yaw',
-        default_value='0.0',
-        description='yaw angle of initial orientation, radians')
-
-
-    ################################################################################################
-    # Launch stuff
-
-    # Include Robot State Publisher launch file if enabled
-    robot_state_publisher_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([
-            os.path.join(pkg_share_description, 'launch', 'robot_state_publisher.launch.py')
-        ]),
-        launch_arguments={
-            'jsp_gui': jsp_gui,
-            'use_camera': use_camera,
-            'use_gazebo': use_gazebo,
-            'use_rviz': use_rviz,
-            'use_sim_time': use_sim_time
-        }.items(),
-        condition=IfCondition(use_robot_state_pub)
-    )
-
-    # # Include ROS 2 Controllers launch file if enabled
-    # load_controllers_cmd = IncludeLaunchDescription(
-    #     PythonLaunchDescriptionSource([
-    #         os.path.join(pkg_share_moveit, 'launch', 'load_ros2_controllers.launch.py')
-    #     ]),
-    #     launch_arguments={
-    #         'use_sim_time': use_sim_time
-    #     }.items(),
-    #     condition=IfCondition(load_controllers)
-    # )
-
-    # Set Gazebo model path - include both models directory and ROS packages
-    set_env_vars_resources = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH',
-        gazebo_models_path)
-    
-    # Add ROS packages path so Gazebo can resolve package:// URIs
-    set_env_vars_packages = AppendEnvironmentVariable(
-        'GZ_SIM_RESOURCE_PATH',
-        os.path.dirname(pkg_share_description))
-
-    # Start Gazebo with optimized arguments
-    start_gazebo_cmd = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
-        launch_arguments=[('gz_args', [' -r -v 3 --render-engine ogre2 ', world_path])])
-
-    # Bridge ROS topics and Gazebo messages for establishing communication
-    start_gazebo_ros_bridge_cmd = Node(
-        package='ros_gz_bridge',
-        executable='parameter_bridge',
-        parameters=[{
-            'config_file': default_ros_gz_bridge_config_file_path,
-        }],
-        output='screen'
-    )
-
-    # Includes optimizations to minimize latency and bandwidth when streaming image data
-    start_gazebo_ros_image_bridge_cmd = Node(
-        package='ros_gz_image',
-        executable='image_bridge',
-        arguments=[
-            '/camera_head/depth_image',
-            '/camera_head/image',
-        ],
-        remappings=[
-            ('/camera_head/depth_image', '/camera_head/depth/image_rect_raw'),
-            ('/camera_head/image', '/camera_head/color/image_raw'),
-        ],
-        condition=IfCondition(use_camera)
-    )
-
-    # Spawn the robot
-    start_gazebo_ros_spawner_cmd = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=[
-            '-topic', '/robot_description',
-            '-name', robot_name,
-            '-allow_renaming', 'true',
-            '-x', x,
-            '-y', y,
-            '-z', z,
-            '-R', roll,
-            '-P', pitch,
-            '-Y', yaw
-        ])
-
-
-    ################################################################################################
-    # Launch description
-
-    ld = LaunchDescription()
-
-    # Declare the launch options
-    ld.add_action(declare_robot_name_cmd)
-    ld.add_action(declare_jsp_gui_cmd)
-    ld.add_action(declare_load_controllers_cmd)
-    ld.add_action(declare_use_camera_cmd)
-    ld.add_action(declare_use_gazebo_cmd)
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_use_robot_state_pub_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-    ld.add_action(declare_world_cmd)
-
-    # Add pose arguments
-    ld.add_action(declare_x_cmd)
-    ld.add_action(declare_y_cmd)
-    ld.add_action(declare_z_cmd)
-    ld.add_action(declare_roll_cmd)
-    ld.add_action(declare_pitch_cmd)
-    ld.add_action(declare_yaw_cmd)
-
-    # Add the actions to the launch description
-    ld.add_action(set_env_vars_resources)
-    ld.add_action(set_env_vars_packages)
-    ld.add_action(robot_state_publisher_cmd)
-    # ld.add_action(load_controllers_cmd)
-    ld.add_action(start_gazebo_cmd)
-    ld.add_action(start_gazebo_ros_bridge_cmd)
-    ld.add_action(start_gazebo_ros_image_bridge_cmd)
-    ld.add_action(start_gazebo_ros_spawner_cmd)
-
-    return ld
--- a/src/core_gazebo/worlds/empty.world
+++ b/src/core_gazebo/worlds/empty.world
@@ -1,89 +0,0 @@
-<?xml version="1.0" ?>
-
-<sdf version="1.6">
-  <world name="default">
-
-    <!-- Plugin for simulating physics -->
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-      <engine>
-        <filename>libgz-physics-bullet-featherstone-plugin.so</filename>
-      </engine>
-    </plugin>
-
-    <!-- Plugin for handling user commands -->
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-
-    <!-- Plugin for broadcasting scene updates -->
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <!-- Plugin for handling sensors like the LIDAR -->
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <!-- Plugin for IMU -->
-    <plugin filename="gz-sim-imu-system"
-      name="gz::sim::systems::Imu">
-    </plugin>
-
-    <!-- To add realistic gravity, do: 0.0 0.0 -9.8, otherwise do 0.0 0.0 0.0 -->
-    <gravity>0.0 0.0 -9.8</gravity>
-
-    <!-- Include a model of the Sun from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
-      </uri>
-    </include>
-
-    <!-- Local Ground Plane with modified friction -->
-    <model name="ground_plane">
-      <static>true</static>
-      <link name="link">
-        <collision name="collision">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-            </plane>
-          </geometry>
-          <surface>
-            <friction>
-              <torsional>
-                <coefficient>0.0</coefficient>
-              </torsional>
-            </friction>
-          </surface>
-        </collision>
-        <visual name="visual">
-          <geometry>
-            <plane>
-              <normal>0 0 1</normal>
-              <size>100 100</size>
-            </plane>
-          </geometry>
-          <material>
-            <ambient>0.8 0.8 0.8 1</ambient>
-            <diffuse>0.8 0.8 0.8 1</diffuse>
-            <specular>0.8 0.8 0.8 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
-
-    <!-- Define scene properties -->
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-  </world>
-</sdf>
--- a/src/core_gazebo/worlds/pick_and_place_demo.world
+++ b/src/core_gazebo/worlds/pick_and_place_demo.world
@@ -1,83 +0,0 @@
-<?xml version="1.0" ?>
-
-<sdf version="1.7">
-  <world name="default">
-
-    <!-- Plugin for simulating physics -->
-    <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
-    </plugin>
-
-    <!-- Plugin for handling user commands -->
-    <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
-    </plugin>
-
-    <!-- Plugin for broadcasting scene updates -->
-    <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
-    </plugin>
-
-    <!-- Plugin for sensor handling -->
-    <plugin
-      filename="gz-sim-sensors-system"
-      name="gz::sim::systems::Sensors">
-      <render_engine>ogre2</render_engine>
-    </plugin>
-
-    <!-- To add realistic gravity, do: 0.0 0.0 -9.8, otherwise do 0.0 0.0 0.0 -->
-    <gravity>0.0 0.0 -9.8</gravity>
-
-    <!-- Include a model of the Sun from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
-      </uri>
-    </include>
-
-    <!-- Include a model of the Ground Plane from an external URI -->
-    <include>
-      <uri>
-        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
-      </uri>
-    </include>
-
-    <!-- Include the cylinder model -->
-    <!-- <include>
-      <uri>model://red_cylinder</uri>
-      <name>red_cylinder</name>
-      <pose>0.22 0.12 0.175 0 0 0</pose>
-    </include> -->
-
-    <!-- Include the other objects -->
-
-    <!-- <include>
-      <uri>model://mustard</uri>
-      <pose>0.7 0.15 0.08 0 0 0</pose>
-    </include>
-
-    <include>
-      <uri>model://cheezit_big_original</uri>
-      <pose>0.64 0.23 0.17 1.571 0 0</pose>
-    </include>
-
-    <include>
-      <uri>model://cardboard_box</uri>
-      <pose>0.65 0.60 0.3 0 0 0.5</pose>
-    </include>
-
-    <include>
-      <uri>model://coke_can</uri>
-      <pose>0.5 0.15 0.15 0 0 2.3</pose>
-    </include> -->
-
-    <!-- Define scene properties -->
-    <scene>
-      <shadows>false</shadows>
-    </scene>
-
-  </world>
-</sdf>
--- a/src/core_pkg/core_pkg/core_headless.py
+++ b/src/core_pkg/core_pkg/core_headless.py
@@ -13,21 +13,24 @@ import os

 import importlib
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import CoreControl
+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()
+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)
+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...")
@@ -39,23 +42,25 @@ class Headless(Node):
                    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()}')
-        
+        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
+        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()
@@ -68,7 +73,7 @@ class Headless(Node):
            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...")
@@ -82,7 +87,7 @@ class Headless(Node):
            # 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
@@ -91,15 +96,17 @@ class Headless(Node):
        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}'
+        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__':
+
+if __name__ == "__main__":
    main()
--- a/src/core_pkg/core_pkg/core_node.py
+++ b/src/core_pkg/core_pkg/core_node.py
@@ -14,13 +14,13 @@ import sys
 import threading
 import glob
 from scipy.spatial.transform import Rotation
-from math import copysign
+from math import copysign, pi

 from std_msgs.msg import String, Header
-from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
+from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
 from geometry_msgs.msg import TwistStamped, Twist
-from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback
-from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
+from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
+from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState


 serial_pub = None
@@ -28,7 +28,7 @@ thread = None

 CORE_WHEELBASE = 0.836  # meters
 CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100  # Clucky: 100:1, Testbed: 64:1
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1

 control_qos = qos.QoSProfile(
    history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -38,9 +38,23 @@ control_qos = qos.QoSProfile(
    deadline=Duration(seconds=1),
    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    liveliness_lease_duration=Duration(seconds=5),
 )

+# Used to verify the length of an incoming VicCAN feedback message
+# 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 SerialRelay(Node):
    def __init__(self):
@@ -48,75 +62,110 @@ class SerialRelay(Node):
        super().__init__("core_node")

        # Launch mode -- anchor vs core
-        self.declare_parameter('launch_mode', 'core')
-        self.launch_mode = self.get_parameter('launch_mode').value
+        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

        # 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)
+        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(String, '/anchor/core/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/core/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)

        # Control

        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, qos_profile=control_qos)
+        self.cmd_vel_sub_ = self.create_subscription(
+            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
+        )
        # manual twist -- [-1, 1] rather than real units
-        self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
+        self.twist_man_sub_ = self.create_subscription(
+            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+        )
        # manual flags -- brake mode and max duty cycle
-        self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
-        self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        self.control_state_sub_ = self.create_subscription(
+            CoreCtrlState,
+            "/core/control/state",
+            self.control_state_callback,
+            qos_profile=control_qos,
+        )
+        self.twist_max_duty = (
+            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        )

        # Feedback

        # Consolidated and organized core feedback
-        self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', 10)
+        self.feedback_new_pub_ = self.create_publisher(
+            NewCoreFeedback,
+            "/core/feedback_new",
+            qos_profile=qos.qos_profile_sensor_data,
+        )
        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)  # TODO: not sure about this
+        self.telemetry_pub_timer = self.create_timer(
+            1.0, self.publish_feedback
+        )  # 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', 10)
+        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(NavSatFix, '/gps/fix', 10)
+        self.gps_pub_ = self.create_publisher(
+            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, '/core/baro', 10)
+        self.baro_pub_ = self.create_publisher(
+            Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
+        )
        self.baro_state = Barometer()
        self.baro_state.header.frame_id = "core_bmp388"

        # Old
-        
+
        # /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
+        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.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)
-
+        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':
+        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()
@@ -125,7 +174,7 @@ class SerialRelay(Node):
                    try:
                        # connect and send a ping command
                        ser = serial.Serial(port, 115200, timeout=1)
-                        #(f"Checking port {port}...")
+                        # (f"Checking port {port}...")
                        ser.write(b"ping\n")
                        response = ser.read_until("\n")  # type: ignore

@@ -140,17 +189,16 @@ class SerialRelay(Node):
                        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
@@ -159,15 +207,15 @@ class SerialRelay(Node):

        try:
            while rclpy.ok():
-                if self.launch_mode == 'core':
-                    self.read_MCU() # Check the MCU for updates
+                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}")
@@ -197,8 +245,8 @@ class SerialRelay(Node):
    def scale_duty(self, value: float, max_speed: float):
        leftMin = -1
        leftMax = 1
-        rightMin = -max_speed/100.0
-        rightMax = max_speed/100.0
+        rightMin = -max_speed / 100.0
+        rightMax = max_speed / 100.0

        # Figure out how 'wide' each range is
        leftSpan = leftMax - leftMin
@@ -211,17 +259,29 @@ class SerialRelay(Node):
        return str(rightMin + (valueScaled * rightSpan))

    def send_controls(self, msg: CoreControl):
-        if(msg.turn_to_enable):
-            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
+        if msg.turn_to_enable:
+            command = (
+                "can_relay_tovic,core,41,"
+                + str(msg.turn_to)
+                + ","
+                + str(msg.turn_to_timeout)
+                + "\n"
+            )
        else:
-            command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
+            command = (
+                "can_relay_tovic,core,19,"
+                + self.scale_duty(msg.left_stick, msg.max_speed)
+                + ","
+                + self.scale_duty(msg.right_stick, msg.max_speed)
+                + "\n"
+            )
        self.send_cmd(command)

        # Brake mode
-        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
+        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + "\n"
        self.send_cmd(command)

-        #print(f"[Sys] Relaying: {command}")
+        # print(f"[Sys] Relaying: {command}")

    def cmd_vel_callback(self, msg: TwistStamped):
        linear = msg.twist.linear.x
@@ -232,20 +292,22 @@ class SerialRelay(Node):

        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        
+
        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])

    def twist_man_callback(self, msg: Twist):
        linear = msg.linear.x  # [-1 1] for forward/back from left stick y
        angular = msg.angular.z  # [-1 1] for left/right from right stick x

-        if (linear < 0):  # reverse turning direction when going backwards (WIP)
+        if linear < 0:  # reverse turning direction when going backwards (WIP)
            angular *= -1

        if abs(linear) > 1 or abs(angular) > 1:
            # if speed is greater than 1, then there is a problem
            # make it look like a problem and don't just run away lmao
-            linear = copysign(0.25, linear)  # 0.25 duty cycle in direction of control (hopefully slow)
+            linear = copysign(
+                0.25, linear
+            )  # 0.25 duty cycle in direction of control (hopefully slow)
            angular = copysign(0.25, angular)

        duty_left = linear - angular
@@ -256,8 +318,12 @@ class SerialRelay(Node):

        # Apply max duty cycle
        # Joysticks provide values [-1, 1] rather than real units
-        duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty)
-        duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
+        duty_left = map_range(
+            duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty
+        )
+        duty_right = map_range(
+            duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty
+        )

        self.send_viccan(19, [duty_left, duty_right])

@@ -269,24 +335,24 @@ class SerialRelay(Node):
        self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this

    def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor':
-            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()#Convert to std_msg string
+        if self.launch_mode == "anchor":
+            # 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':
+        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(VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
-            mcu_name="core",
-            command_id=cmd_id,
-            data=data
-        ))
-
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
+                mcu_name="core",
+                command_id=cmd_id,
+                data=data,
+            )
+        )

    def anchor_feedback(self, msg: String):
        output = msg.data
@@ -350,121 +416,150 @@ class SerialRelay(Node):
            return
        self.feedback_new_state.header.stamp = self.get_clock().now().to_msg()
        self.feedback_new_pub_.publish(self.feedback_new_state)
-        #self.get_logger().info(f"[Core Anchor] {msg}")
+        # self.get_logger().info(f"[Core Anchor] {msg}")

    def relay_fromvic(self, msg: VicCAN):
        # Assume that the message is coming from Core
        # skill diff if not
-        
-        # TODO: add len(msg.data) checks to each feedback message

-        # GNSS
-        if msg.command_id == 48:  # GNSS Latitude
-            self.gps_state.latitude = float(msg.data[0])
-        elif msg.command_id == 49:  # GNSS Longitude
-            self.gps_state.longitude = float(msg.data[0])
-        elif msg.command_id == 50:  # GNSS Satellite count and altitude
-            self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
-            self.gps_state.altitude = float(msg.data[1])
-            self.gps_state.header.stamp = msg.header.stamp
-            self.gps_pub_.publish(self.gps_state)
-        # IMU
-        elif msg.command_id == 51:  # Gyro x, y, z, and imu calibration
-            self.feedback_new_state.imu_calib = round(float(msg.data[3]))
-            self.imu_state.angular_velocity.x = float(msg.data[0])
-            self.imu_state.angular_velocity.y = float(msg.data[1])
-            self.imu_state.angular_velocity.z = float(msg.data[2])
-            self.imu_state.header.stamp = msg.header.stamp
-        elif msg.command_id == 52:  # Accel x, y, z, heading
-            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])
-            # Deal with quaternion
-            r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
-            q = r.as_quat()
-            self.imu_state.orientation.x = q[0]
-            self.imu_state.orientation.y = q[1]
-            self.imu_state.orientation.z = q[2]
-            self.imu_state.orientation.w = q[3]
-            self.imu_state.header.stamp = msg.header.stamp
-            self.imu_pub_.publish(self.imu_state)
-        # REV Motors
-        elif msg.command_id == 53:  # REV SPARK MAX feedback
-            motorId = round(float(msg.data[0]))
-            temp = float(msg.data[1]) / 10.0
-            voltage = float(msg.data[2]) / 10.0
-            current = float(msg.data[3]) / 10.0
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.temperature = temp
-                self.feedback_new_state.fl_motor.voltage = voltage
-                self.feedback_new_state.fl_motor.current = current
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.temperature = temp
-                self.feedback_new_state.bl_motor.voltage = voltage
-                self.feedback_new_state.bl_motor.current = current
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.temperature = temp
-                self.feedback_new_state.fr_motor.voltage = voltage
-                self.feedback_new_state.fr_motor.current = current
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.temperature = temp
-                self.feedback_new_state.br_motor.voltage = voltage
-                self.feedback_new_state.br_motor.current = current
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-            self.feedback_new_pub_.publish(self.feedback_new_state)
-        # Board voltage
-        elif msg.command_id == 54:  # Voltages batt, 12, 5, 3, all * 100
-            self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
-            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
-        # Baro
-        elif msg.command_id == 56:  # BMP temperature, altitude, pressure
-            self.baro_state.temperature = float(msg.data[0])
-            self.baro_state.altitude = float(msg.data[1])
-            self.baro_state.pressure = float(msg.data[2])
-            self.baro_state.header.stamp = msg.header.stamp
-            self.baro_pub_.publish(self.baro_state)
-        # REV Motors (pos and vel)
-        elif msg.command_id == 58:  # REV position and velocity
-            motorId = round(float(msg.data[0]))
-            position = float(msg.data[1])
-            velocity = float(msg.data[2])
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.position = position
-                self.feedback_new_state.fl_motor.velocity = velocity
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.position = position
-                self.feedback_new_state.bl_motor.velocity = velocity
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.position = position
-                self.feedback_new_state.fr_motor.velocity = velocity
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.position = position
-                self.feedback_new_state.br_motor.velocity = velocity
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-        else:
-            return
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_msg_len_dict:
+            expected_len = 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

+        match msg.command_id:
+            # GNSS
+            case 48:  # GNSS Latitude
+                self.gps_state.latitude = float(msg.data[0])
+            case 49:  # GNSS Longitude
+                self.gps_state.longitude = float(msg.data[0])
+            case 50:  # GNSS Satellite count and altitude
+                self.gps_state.status.status = (
+                    NavSatStatus.STATUS_FIX
+                    if int(msg.data[0]) >= 3
+                    else NavSatStatus.STATUS_NO_FIX
+                )
+                self.gps_state.altitude = float(msg.data[1])
+                self.gps_state.header.stamp = msg.header.stamp
+                self.gps_pub_.publish(self.gps_state)
+            # IMU
+            case 51:  # Gyro x, y, z, and imu calibration
+                self.feedback_new_state.imu_calib = round(float(msg.data[3]))
+                self.imu_state.angular_velocity.x = float(msg.data[0])
+                self.imu_state.angular_velocity.y = float(msg.data[1])
+                self.imu_state.angular_velocity.z = float(msg.data[2])
+                self.imu_state.header.stamp = msg.header.stamp
+            case 52:  # Accel x, y, z, heading
+                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])
+                # Deal with quaternion
+                r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
+                q = r.as_quat()
+                self.imu_state.orientation.x = q[0]
+                self.imu_state.orientation.y = q[1]
+                self.imu_state.orientation.z = q[2]
+                self.imu_state.orientation.w = q[3]
+                self.imu_state.header.stamp = msg.header.stamp
+                self.imu_pub_.publish(self.imu_state)
+            # REV Motors
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(float(msg.data[0]))
+                temp = float(msg.data[1]) / 10.0
+                voltage = float(msg.data[2]) / 10.0
+                current = float(msg.data[3]) / 10.0
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                    case _:
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 53 with invalid motorId {motorId}"
+                        )
+                        return
+
+                if motor:
+                    motor.temperature = temp
+                    motor.voltage = voltage
+                    motor.current = current
+                    motor.header.stamp = msg.header.stamp
+
+                self.feedback_new_pub_.publish(self.feedback_new_state)
+            # Board voltage
+            case 54:  # Voltages batt, 12, 5, 3, all * 100
+                self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
+                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
+            # Baro
+            case 56:  # BMP temperature, altitude, pressure
+                self.baro_state.temperature = float(msg.data[0])
+                self.baro_state.altitude = float(msg.data[1])
+                self.baro_state.pressure = float(msg.data[2])
+                self.baro_state.header.stamp = msg.header.stamp
+                self.baro_pub_.publish(self.baro_state)
+            # REV Motors (pos and vel)
+            case 58:  # REV position and velocity
+                motorId = round(float(msg.data[0]))
+                position = float(msg.data[1])
+                velocity = float(msg.data[2])
+                joint_state_msg = (
+                    JointState()
+                )  # TODO: not sure if all motors should be in each message or not
+                joint_state_msg.position = [
+                    position * (2 * pi) / CORE_GEAR_RATIO
+                ]  # revolutions to radians
+                joint_state_msg.velocity = [
+                    velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
+                ]  # RPM to rad/s
+
+                motor: RevMotorState | None = None
+
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                        joint_state_msg.name = ["fl_motor_joint"]
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                        joint_state_msg.name = ["bl_motor_joint"]
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                        joint_state_msg.name = ["fr_motor_joint"]
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                        joint_state_msg.name = ["br_motor_joint"]
+                    case _:
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
+                        )
+                        return
+
+                joint_state_msg.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(joint_state_msg)
+            case _:
+                return

    def publish_feedback(self):
-        #self.get_logger().info(f"[Core] {self.core_feedback}")
+        # 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:
@@ -479,20 +574,26 @@ def myexcepthook(type, value, tb):
    if serial_pub:
        serial_pub.cleanup()

-def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float):
+
+def map_range(
+    value: float, in_min: float, in_max: float, out_min: float, out_max: float
+):
    return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min


 def main(args=None):
-   rclpy.init(args=args)
-   sys.excepthook = myexcepthook
+    rclpy.init(args=args)
+    sys.excepthook = myexcepthook

-   global serial_pub
+    global serial_pub

-   serial_pub = SerialRelay()
-   serial_pub.run()
+    serial_pub = SerialRelay()
+    serial_pub.run()

-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
+
+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()
--- a/src/core_pkg/core_pkg/core_ptz.py
+++ b/src/core_pkg/core_pkg/core_ptz.py
@@ -9,7 +9,7 @@ import threading
 import time

 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import PtzControl, PtzFeedback
+from astra_msgs.msg import PtzControl, PtzFeedback

 # Import the SIYI SDK
 from core_pkg.siyi_sdk import (
@@ -18,55 +18,63 @@ from core_pkg.siyi_sdk import (
    DataStreamType,
    DataStreamFrequency,
    SingleAxis,
-    AttitudeData
+    AttitudeData,
 )

+
 class PtzNode(Node):
    def __init__(self):
        # Initialize node with name
        super().__init__("core_ptz")

        # Declare parameters
-        self.declare_parameter('camera_ip', '192.168.1.9')
-        self.declare_parameter('camera_port', 37260)
-        
+        self.declare_parameter("camera_ip", "192.168.1.9")
+        self.declare_parameter("camera_port", 37260)
+
        # Get parameters
-        self.camera_ip = self.get_parameter('camera_ip').value
-        self.camera_port = self.get_parameter('camera_port').value
-        
-        self.get_logger().info(f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}")
-        
+        self.camera_ip = self.get_parameter("camera_ip").value
+        self.camera_port = self.get_parameter("camera_port").value
+
+        self.get_logger().info(
+            f"PTZ camera IP: {self.camera_ip} Port: {self.camera_port}"
+        )
+
        # Create a camera instance
        self.camera = None
-        self.camera_connected = False # This flag is still managed but not used to gate commands
+        self.camera_connected = (
+            False  # This flag is still managed but not used to gate commands
+        )
        self.loop = None
        self.thread_pool = None
-        
+
        # Create publishers
-        self.feedback_pub = self.create_publisher(PtzFeedback, '/ptz/feedback', 10)
-        self.debug_pub = self.create_publisher(String, '/ptz/debug', 10)
-        
+        self.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
+        self.debug_pub = self.create_publisher(String, "/ptz/debug", 10)
+
        # Create subscribers
        self.control_sub = self.create_subscription(
-            PtzControl, '/ptz/control', self.handle_control_command, 10)
-            
+            PtzControl, "/ptz/control", self.handle_control_command, 10
+        )
+
        # Create timers
        self.connection_timer = self.create_timer(5.0, self.check_camera_connection)
        self.last_data_time = time.time()
        self.health_check_timer = self.create_timer(2.0, self.check_camera_health)
-        
+
        # Create feedback message
        self.feedback_msg = PtzFeedback()
-        self.feedback_msg.connected = False # This will reflect the actual connection state
+        self.feedback_msg.connected = (
+            False  # This will reflect the actual connection state
+        )
        self.feedback_msg.error_msg = "Initializing"
-        
+
        # Flags for async operations
        self.shutdown_requested = False
-        
+
        # Set up asyncio event loop in a separate thread
        self.thread_pool = ThreadPoolExecutor(max_workers=1)
        self.loop = asyncio.new_event_loop()
-        
+
        # Connect to camera on startup
        self.connect_task = self.thread_pool.submit(
            self.run_async_func, self.connect_to_camera()
@@ -77,28 +85,27 @@ class PtzNode(Node):
        try:
            # Create a new camera instance
            self.camera = SiyiGimbalCamera(ip=self.camera_ip, port=self.camera_port)
-            
+
            # Connect to the camera
            await self.camera.connect()
-            
+
            # Set up data callback
            self.camera.set_data_callback(self.camera_data_callback)
-            
+
            # Request attitude data stream
            await self.camera.send_data_stream_request(
-                DataStreamType.ATTITUDE_DATA,
-                DataStreamFrequency.HZ_10
+                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
            )
-            
+
            # Update connection status
            self.camera_connected = True
            self.feedback_msg.connected = True
            self.feedback_msg.error_msg = ""
-            
+
            self.publish_debug("Camera connected successfully")
-            
+
        except Exception as e:
-            self.camera_connected = False 
+            self.camera_connected = False
            self.feedback_msg.connected = False
            self.feedback_msg.error_msg = f"Connection error: {str(e)}"
            self.publish_debug(f"Camera connection failed: {str(e)}")
@@ -108,8 +115,12 @@ class PtzNode(Node):
        # Update last_data_time regardless of self.camera_connected,
        # as data might arrive during a brief reconnect window.
        self.last_data_time = time.time()
-        if self.camera_connected: # Only process for feedback if we believe we are connected
-            if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
+        if (
+            self.camera_connected
+        ):  # Only process for feedback if we believe we are connected
+            if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
+                data, AttitudeData
+            ):
                self.feedback_msg.yaw = data.yaw
                self.feedback_msg.pitch = data.pitch
                self.feedback_msg.roll = data.roll
@@ -123,36 +134,39 @@ class PtzNode(Node):
                    debug_str = f"Camera data: CMD_ID={cmd_id.name}, Data="
                else:
                    debug_str = f"Camera data: CMD_ID={cmd_id}, Data="
-                
+
                if isinstance(data, bytes):
                    debug_str += data.hex()
                else:
                    debug_str += str(data)
                self.get_logger().debug(debug_str)

-
    def check_camera_connection(self):
        """Periodically check camera connection and attempt to reconnect if needed."""
        if not self.camera_connected and not self.shutdown_requested:
            self.publish_debug("Attempting to reconnect to camera...")
            if self.camera:
                try:
-                    if self.camera.is_connected: # SDK's internal connection state
-                         self.run_async_func(self.camera.disconnect())
+                    if self.camera.is_connected:  # SDK's internal connection state
+                        self.run_async_func(self.camera.disconnect())
                except Exception as e:
-                    self.get_logger().debug(f"Error during pre-reconnect disconnect: {e}")
+                    self.get_logger().debug(
+                        f"Error during pre-reconnect disconnect: {e}"
+                    )
                # self.camera = None # Don't nullify here, connect_to_camera will re-assign or create new
-                
+
            self.connect_task = self.thread_pool.submit(
                self.run_async_func, self.connect_to_camera()
            )

    def check_camera_health(self):
        """Check if we're still receiving data from the camera"""
-        if self.camera_connected: # Only check health if we think we are connected
+        if self.camera_connected:  # Only check health if we think we are connected
            time_since_last_data = time.time() - self.last_data_time
            if time_since_last_data > 5.0:
-                self.publish_debug(f"No camera data for {time_since_last_data:.1f}s, marking as disconnected.")
+                self.publish_debug(
+                    f"No camera data for {time_since_last_data:.1f}s, marking as disconnected."
+                )
                self.camera_connected = False
                self.feedback_msg.connected = False
                self.feedback_msg.error_msg = "Connection stale (no data)"
@@ -161,19 +175,20 @@ class PtzNode(Node):
    def handle_control_command(self, msg):
        """Handle incoming control commands."""
        # Removed: if not self.camera_connected
-        if not self.camera: # Still check if camera object exists
-            self.get_logger().warning("Camera object not initialized, ignoring control command")
+        if not self.camera:  # Still check if camera object exists
+            self.get_logger().warning(
+                "Camera object not initialized, ignoring control command"
+            )
            return
-            
-        self.thread_pool.submit(
-            self.run_async_func,
-            self.process_control_command(msg)
-        )
+
+        self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))

    async def process_control_command(self, msg):
        """Process and send the control command to the camera."""
-        if not self.camera: 
-            self.get_logger().error("Process control command called but camera object is None.")
+        if not self.camera:
+            self.get_logger().error(
+                "Process control command called but camera object is None."
+            )
            return
        try:
            # The SDK's send_... methods will raise RuntimeError if not connected.
@@ -182,43 +197,55 @@ class PtzNode(Node):
                self.get_logger().info("Attempting to reset camera to center position")
                await self.camera.send_attitude_angles_command(0.0, 0.0)
                return
-                
+
            if msg.control_mode == 0:
                turn_yaw = max(-100, min(100, int(msg.turn_yaw)))
                turn_pitch = max(-100, min(100, int(msg.turn_pitch)))
-                self.get_logger().debug(f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}")
+                self.get_logger().debug(
+                    f"Attempting rotation: yaw_speed={turn_yaw}, pitch_speed={turn_pitch}"
+                )
                await self.camera.send_rotation_command(turn_yaw, turn_pitch)
-                
+
            elif msg.control_mode == 1:
                yaw = max(-135.0, min(135.0, msg.yaw))
                pitch = max(-90.0, min(90.0, msg.pitch))
-                self.get_logger().debug(f"Attempting absolute angles: yaw={yaw}, pitch={pitch}")
+                self.get_logger().debug(
+                    f"Attempting absolute angles: yaw={yaw}, pitch={pitch}"
+                )
                await self.camera.send_attitude_angles_command(yaw, pitch)
-                
+
            elif msg.control_mode == 2:
                axis = SingleAxis.YAW if msg.axis_id == 0 else SingleAxis.PITCH
                angle = msg.angle
-                self.get_logger().debug(f"Attempting single axis: axis={axis.name}, angle={angle}")
+                self.get_logger().debug(
+                    f"Attempting single axis: axis={axis.name}, angle={angle}"
+                )
                await self.camera.send_single_axis_attitude_command(angle, axis)

-            elif msg.control_mode == 3: 
+            elif msg.control_mode == 3:
                zoom_level = msg.zoom_level
-                self.get_logger().debug(f"Attempting absolute zoom: level={zoom_level}x")
+                self.get_logger().debug(
+                    f"Attempting absolute zoom: level={zoom_level}x"
+                )
                await self.camera.send_absolute_zoom_command(zoom_level)
-                
-            if hasattr(msg, 'stream_type') and hasattr(msg, 'stream_freq'):
-                if msg.stream_type > 0 and msg.stream_freq >= 0: 
+
+            if hasattr(msg, "stream_type") and hasattr(msg, "stream_freq"):
+                if msg.stream_type > 0 and msg.stream_freq >= 0:
                    try:
                        stream_type = DataStreamType(msg.stream_type)
                        stream_freq = DataStreamFrequency(msg.stream_freq)
                        self.get_logger().info(
                            f"Attempting to set data stream: type={stream_type.name}, freq={stream_freq.name}"
                        )
-                        await self.camera.send_data_stream_request(stream_type, stream_freq)
+                        await self.camera.send_data_stream_request(
+                            stream_type, stream_freq
+                        )
                    except ValueError:
-                        self.get_logger().error("Invalid stream type or frequency values in control message")
-            
-        except RuntimeError as e: # Catch SDK's "not connected" errors
+                        self.get_logger().error(
+                            "Invalid stream type or frequency values in control message"
+                        )
+
+        except RuntimeError as e:  # Catch SDK's "not connected" errors
            self.get_logger().warning(f"SDK command failed (likely not connected): {e}")
            # self.camera_connected will be updated by health/connection checks
            # self.feedback_msg.error_msg = f"Command failed: {str(e)}" # Already set by health check
@@ -226,58 +253,64 @@ class PtzNode(Node):
        except Exception as e:
            self.get_logger().error(f"Error processing control command: {e}")
            self.feedback_msg.error_msg = f"Control error: {str(e)}"
-            self.feedback_pub.publish(self.feedback_msg) # Publish for other errors
+            self.feedback_pub.publish(self.feedback_msg)  # Publish for other errors

    def publish_debug(self, message_text):
        """Publish debug message."""
        msg = String()
-        msg.data = f"[{self.get_clock().now().nanoseconds / 1e9:.2f}] PTZ Node: {message_text}"
+        msg.data = (
+            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().info(message_text)

    def run_async_func(self, coro):
        """Run an async function in the event loop."""
        if self.loop and self.loop.is_running():
            try:
-                return asyncio.run_coroutine_threadsafe(coro, self.loop).result(timeout=5.0) # Added timeout
+                return asyncio.run_coroutine_threadsafe(coro, self.loop).result(
+                    timeout=5.0
+                )  # Added timeout
            except asyncio.TimeoutError:
                self.get_logger().warning(f"Async function {coro.__name__} timed out.")
                return None
            except Exception as e:
-                self.get_logger().error(f"Exception in run_async_func for {coro.__name__}: {e}")
+                self.get_logger().error(
+                    f"Exception in run_async_func for {coro.__name__}: {e}"
+                )
                return None
        else:
-            self.get_logger().warning("Asyncio loop not running, cannot execute coroutine.")
+            self.get_logger().warning(
+                "Asyncio loop not running, cannot execute coroutine."
+            )
            return None

-
    async def shutdown_node_async(self):
        """Perform clean shutdown of camera connection."""
        self.shutdown_requested = True
        self.get_logger().info("Async shutdown initiated...")
-        if self.camera and self.camera.is_connected: # Check SDK's connection state
+        if self.camera and self.camera.is_connected:  # Check SDK's connection state
            try:
                self.get_logger().info("Disabling data stream...")
                await self.camera.send_data_stream_request(
-                    DataStreamType.ATTITUDE_DATA, 
-                    DataStreamFrequency.DISABLE
+                    DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
                )
-                await asyncio.sleep(0.1) 
-                
+                await asyncio.sleep(0.1)
+
                self.get_logger().info("Disconnecting from camera...")
                await self.camera.disconnect()
                self.get_logger().info("Disconnected from camera successfully.")
-                
+
            except Exception as e:
                self.get_logger().error(f"Error during camera shutdown: {e}")
-        self.camera_connected = False # Update node's flag
+        self.camera_connected = False  # Update node's flag
        self.feedback_msg.connected = False
        self.feedback_msg.error_msg = "Shutting down"

    def cleanup(self):
        """Clean up resources."""
        self.get_logger().info("PTZ node cleanup initiated.")
-        self.shutdown_requested = True 
+        self.shutdown_requested = True

        if self.connection_timer:
            self.connection_timer.cancel()
@@ -287,31 +320,38 @@ class PtzNode(Node):
        if self.loop and self.thread_pool:
            if self.loop.is_running():
                try:
-                    future = asyncio.run_coroutine_threadsafe(self.shutdown_node_async(), self.loop)
-                    future.result(timeout=5) 
+                    future = asyncio.run_coroutine_threadsafe(
+                        self.shutdown_node_async(), self.loop
+                    )
+                    future.result(timeout=5)
                except Exception as e:
-                    self.get_logger().error(f"Error during async shutdown in cleanup: {e}")
-            
+                    self.get_logger().error(
+                        f"Error during async shutdown in cleanup: {e}"
+                    )
+
            self.get_logger().info("Shutting down thread pool executor...")
            self.thread_pool.shutdown(wait=True)
-            
+
            if self.loop.is_running():
                self.get_logger().info("Stopping asyncio event loop...")
                self.loop.call_soon_threadsafe(self.loop.stop)
-            
+
            self.get_logger().info("PTZ node resources cleaned up.")
        else:
-            self.get_logger().warning("Loop or thread_pool not initialized, skipping parts of cleanup.")
+            self.get_logger().warning(
+                "Loop or thread_pool not initialized, skipping parts of cleanup."
+            )


 def main(args=None):
    """Main function."""
    rclpy.init(args=args)
-    
+
    ptz_node = PtzNode()
-    
+
    asyncio_thread = None
-    if ptz_node.loop: 
+    if ptz_node.loop:
+
        def run_event_loop(loop):
            asyncio.set_event_loop(loop)
            try:
@@ -322,14 +362,12 @@ def main(args=None):
                # or an unhandled exception within a task scheduled on the loop.
                if not loop.is_closed():
                    loop.close()
-        
+
        asyncio_thread = threading.Thread(
-            target=run_event_loop, 
-            args=(ptz_node.loop,),
-            daemon=True 
+            target=run_event_loop, args=(ptz_node.loop,), daemon=True
        )
        asyncio_thread.start()
-    
+
    try:
        rclpy.spin(ptz_node)
    except KeyboardInterrupt:
@@ -338,18 +376,18 @@ def main(args=None):
        ptz_node.get_logger().info("SystemExit received, shutting down...")
    finally:
        ptz_node.get_logger().info("Initiating final cleanup...")
-        ptz_node.cleanup() # This will stop the loop and shutdown the executor
-        
+        ptz_node.cleanup()  # This will stop the loop and shutdown the executor
+
        if asyncio_thread and asyncio_thread.is_alive():
-             # The loop should have been stopped by cleanup. We just join the thread.
-             ptz_node.get_logger().info("Waiting for asyncio thread to join...")
-             asyncio_thread.join(timeout=5) 
-             if asyncio_thread.is_alive():
-                 ptz_node.get_logger().warning("Asyncio thread did not join cleanly.")
-        
+            # The loop should have been stopped by cleanup. We just join the thread.
+            ptz_node.get_logger().info("Waiting for asyncio thread to join...")
+            asyncio_thread.join(timeout=5)
+            if asyncio_thread.is_alive():
+                ptz_node.get_logger().warning("Asyncio thread did not join cleanly.")
+
        rclpy.shutdown()
        ptz_node.get_logger().info("ROS shutdown complete.")


-if __name__ == '__main__':
+if __name__ == "__main__":
    main()
--- a/src/core_pkg/core_pkg/siyi_sdk/siyi.py
+++ b/src/core_pkg/core_pkg/siyi_sdk/siyi.py
@@ -110,9 +110,7 @@ class SiyiGimbalCamera:

    MAX_A8_MINI_ZOOM = 6.0  # Maximum zoom for A8 mini

-    def __init__(
-        self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2
-    ):
+    def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
        self.ip = ip
        self.port = port
        self.heartbeat_interval = heartbeat_interval
@@ -124,9 +122,7 @@ class SiyiGimbalCamera:

    async def connect(self) -> None:
        try:
-            self.reader, self.writer = await asyncio.open_connection(
-                self.ip, self.port
-            )
+            self.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
            self.is_connected = True
            asyncio.create_task(self.heartbeat_loop())
            asyncio.create_task(self._data_stream_listener())
@@ -158,9 +154,7 @@ class SiyiGimbalCamera:
            if self.is_connected:
                await self.disconnect()

-    def _build_packet_header(
-        self, cmd_id: CommandID, data_len: int
-    ) -> bytearray:
+    def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
        """Helper to build the common packet header."""
        packet = bytearray()
        packet.extend(b"\x55\x66")  # STX
@@ -179,15 +173,11 @@ class SiyiGimbalCamera:

    def _build_rotation_packet(self, turn_yaw: int, turn_pitch: int) -> bytes:
        data_len = 2
-        packet = self._build_packet_header(
-            CommandID.ROTATION_CONTROL, data_len
-        )
+        packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
        packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
        return self._finalize_packet(packet)

-    async def send_rotation_command(
-        self, turn_yaw: int, turn_pitch: int
-    ) -> None:
+    async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
        if not self.is_connected or not self.writer:
            raise RuntimeError(
                "Socket is not connected or writer is None, cannot send rotation command."
@@ -199,21 +189,15 @@ class SiyiGimbalCamera:
            f"Sent rotation command with yaw_speed {turn_yaw} and pitch_speed {turn_pitch}"
        )

-    def _build_attitude_angles_packet(
-        self, yaw: float, pitch: float
-    ) -> bytes:
+    def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
        data_len = 4
-        packet = self._build_packet_header(
-            CommandID.ATTITUDE_ANGLES, data_len
-        )
+        packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
        yaw_int = int(round(yaw * 10))
        pitch_int = int(round(pitch * 10))
        packet.extend(struct.pack("<hh", yaw_int, pitch_int))
        return self._finalize_packet(packet)

-    async def send_attitude_angles_command(
-        self, yaw: float, pitch: float
-    ) -> None:
+    async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
        if not self.is_connected or not self.writer:
            raise RuntimeError(
                "Socket is not connected or writer is None, cannot send attitude angles command."
@@ -221,17 +205,13 @@ class SiyiGimbalCamera:
        packet = self._build_attitude_angles_packet(yaw, pitch)
        self.writer.write(packet)
        await self.writer.drain()
-        logger.debug(
-            f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°"
-        )
+        logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")

    def _build_single_axis_attitude_packet(
        self, angle: float, axis: SingleAxis
    ) -> bytes:
        data_len = 3
-        packet = self._build_packet_header(
-            CommandID.SINGLE_AXIS_CONTROL, data_len
-        )
+        packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
        angle_int = int(round(angle * 10))
        packet.extend(struct.pack("<hB", angle_int, axis.value))
        return self._finalize_packet(packet)
@@ -254,9 +234,7 @@ class SiyiGimbalCamera:
        self, data_type: DataStreamType, data_freq: DataStreamFrequency
    ) -> bytes:
        data_len = 2
-        packet = self._build_packet_header(
-            CommandID.DATA_STREAM_REQUEST, data_len
-        )
+        packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
        packet.append(data_type.value)
        packet.append(data_freq.value)
        return self._finalize_packet(packet)
@@ -279,7 +257,9 @@ class SiyiGimbalCamera:
        data_len = 2
        packet = self._build_packet_header(CommandID.ABSOLUTE_ZOOM, data_len)
        zoom_packet_value = int(round(zoom_level * 10))
-        if not (0 <= zoom_packet_value <= 65535): # Should be caught by clamping earlier
+        if not (
+            0 <= zoom_packet_value <= 65535
+        ):  # Should be caught by clamping earlier
            raise ValueError(
                "Zoom packet value out of uint16_t range after conversion."
            )
@@ -329,24 +309,24 @@ class SiyiGimbalCamera:
        ctrl = await self.reader.readexactly(1)
        data_len_bytes = await self.reader.readexactly(2)
        data_len = struct.unpack("<H", data_len_bytes)[0]
-        seq_bytes = await self.reader.readexactly(2) # Renamed for clarity
+        seq_bytes = await self.reader.readexactly(2)  # Renamed for clarity
        # seq_val = struct.unpack("<H", seq_bytes)[0] # If you need the sequence value
        cmd_id_bytes = await self.reader.readexactly(1)
-        cmd_id_val = cmd_id_bytes[0] # Renamed for clarity
-        
+        cmd_id_val = cmd_id_bytes[0]  # Renamed for clarity
+
        # Protect against excessively large data_len
-        if data_len > 2048: # Arbitrary reasonable limit
+        if data_len > 2048:  # Arbitrary reasonable limit
            raise ValueError(f"Excessive data length received: {data_len}")

        data = await self.reader.readexactly(data_len)
        crc_bytes = await self.reader.readexactly(2)
        received_crc = struct.unpack("<H", crc_bytes)[0]
-        
+
        packet_without_crc = (
            stx + ctrl + data_len_bytes + seq_bytes + cmd_id_bytes + data
        )
        computed_crc = Crc16.calc(packet_without_crc)
-        
+
        if computed_crc != received_crc:
            raise ValueError(
                f"CRC check failed. Expected {computed_crc:04X}, got {received_crc:04X}. "
@@ -374,10 +354,7 @@ class SiyiGimbalCamera:
                        self._data_callback(cmd_id_int, data)
                    continue

-                if (
-                    cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE
-                    and len(data) == 12
-                ):
+                if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
                    try:
                        parsed = AttitudeData.from_bytes(data)
                        if self._data_callback:
@@ -385,9 +362,7 @@ class SiyiGimbalCamera:
                        else:
                            logger.info(f"Received attitude data: {parsed}")
                    except Exception as e:
-                        logger.exception(
-                            f"Failed to parse attitude data: {e}"
-                        )
+                        logger.exception(f"Failed to parse attitude data: {e}")
                        if self._data_callback:
                            self._data_callback(cmd_id_enum, data)
                else:
@@ -410,12 +385,12 @@ class SiyiGimbalCamera:
            except ValueError as e:
                logger.error(f"Packet error in listener: {e}")
                # Consider adding a small delay or a mechanism to resync if this happens frequently
-                await asyncio.sleep(0.1) # Small delay before trying to read again
+                await asyncio.sleep(0.1)  # Small delay before trying to read again
                continue
            except Exception as e:
                logger.exception(f"Unexpected error in data stream listener: {e}")
                # Depending on the error, you might want to break or continue
-                await asyncio.sleep(0.1) # Small delay
+                await asyncio.sleep(0.1)  # Small delay
                continue

    def set_data_callback(
@@ -424,12 +399,14 @@ class SiyiGimbalCamera:
        self._data_callback = callback


-async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
+async def main_sdk_test():  # Renamed to avoid conflict if this file is imported
    gimbal_ip = "192.168.144.25"
    gimbal = SiyiGimbalCamera(gimbal_ip)

    def my_data_handler(cmd_id, data):
-        if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
+        if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(
+            data, AttitudeData
+        ):
            print(
                f"Attitude: Yaw={data.yaw:.1f}, Pitch={data.pitch:.1f}, Roll={data.roll:.1f}"
            )
@@ -460,7 +437,7 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
            print("SDK Test: Setting zoom to 6.0x (A8 mini max)")
            await gimbal.send_absolute_zoom_command(6.0)
            await asyncio.sleep(2)
-            
+
            print("SDK Test: Attempting zoom to 7.0x (should be clamped to 6.0x)")
            await gimbal.send_absolute_zoom_command(7.0)
            await asyncio.sleep(2)
@@ -470,16 +447,19 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
            await asyncio.sleep(2)

            print("SDK Test: Requesting attitude data stream at 5Hz...")
-            await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5)
-            
+            await gimbal.send_data_stream_request(
+                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_5
+            )
+
            print("SDK Test: Listening for data for 10 seconds...")
            await asyncio.sleep(10)

            print("SDK Test: Disabling attitude data stream...")
-            await gimbal.send_data_stream_request(DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE)
+            await gimbal.send_data_stream_request(
+                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
+            )
            await asyncio.sleep(1)

-
    except ConnectionRefusedError:
        print(
            f"SDK Test: Connection to {gimbal_ip} was refused. Is the gimbal on and accessible?"
--- a/src/core_pkg/core_pkg/siyi_sdk/test.py
+++ b/src/core_pkg/core_pkg/siyi_sdk/test.py
@@ -24,7 +24,9 @@ async def main():
        await asyncio.sleep(2)

        # Command 1: Move all the way to the right (using set angles)
-        logger.info("Command 1: Move all the way to the right (using absolute angle control)")
+        logger.info(
+            "Command 1: Move all the way to the right (using absolute angle control)"
+        )
        await camera.send_attitude_angles_command(135.0, 0.0)
        await asyncio.sleep(5)

@@ -35,13 +37,17 @@ async def main():
        await asyncio.sleep(5)

        # Command 3: Stop looking down, then look up (with the single axis)
-        logger.info("Command 3: Stop looking down and start looking up (single axis control)")
+        logger.info(
+            "Command 3: Stop looking down and start looking up (single axis control)"
+        )
        await camera.send_rotation_command(0, 0)
        await camera.send_single_axis_attitude_command(135, SingleAxis.PITCH)
        await asyncio.sleep(5)

        # Command 4: Reset and move all the way to the left (Absolute value).
-        logger.info("Command 4: Move back to the center, and start moving all the way left")
+        logger.info(
+            "Command 4: Move back to the center, and start moving all the way left"
+        )
        await camera.send_attitude_angles_command(-135.0, 0.0)
        await asyncio.sleep(5)

--- a/src/core_pkg/package.xml
+++ b/src/core_pkg/package.xml
@@ -8,7 +8,10 @@
  <license>AGPL-3.0-only</license>

  <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-scipy</depend>
+  <depend>python-crccheck-pip</depend>
+  <depend>astra_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/core_pkg/setup.py
+++ b/src/core_pkg/setup.py
@@ -1,27 +1,26 @@
 from setuptools import find_packages, setup

-package_name = 'core_pkg'
+package_name = "core_pkg"

 setup(
    name=package_name,
-    version='0.0.0',
-    packages=find_packages(exclude=['test']),
+    version="0.0.0",
+    packages=find_packages(exclude=["test"]),
    data_files=[
-        ('share/ament_index/resource_index/packages',
-            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
    ],
-    install_requires=['setuptools'],
+    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="tristan",
+    maintainer_email="tristanmcginnis26@gmail.com",
+    description="Core rover control package to handle command interpretation and embedded interfacing.",
+    license="All Rights Reserved",
    entry_points={
-        'console_scripts': [
+        "console_scripts": [
            "core = core_pkg.core_node:main",
            "headless = core_pkg.core_headless:main",
-            "ptz = core_pkg.core_ptz:main"
+            "ptz = core_pkg.core_ptz:main",
        ],
    },
 )
--- a/src/core_rover_description/config/joint_names_core_rover_description.yaml
+++ b/src/core_rover_description/config/joint_names_core_rover_description.yaml
@@ -1 +0,0 @@
-controller_joint_names: ['', 'right_suspension_joint', 'br_wheel_axis', 'fr_wheel_joint', 'averaging_bar_axis', 'left_suspension_joint', 'bl_wheel_joint', 'fl_wheel_joint', ]
--- a/src/core_rover_description/config/rviz_basic_settings.rviz
+++ b/src/core_rover_description/config/rviz_basic_settings.rviz
@@ -1,229 +0,0 @@
-Panels:
-  - Class: rviz_common/Displays
-    Help Height: 78
-    Name: Displays
-    Property Tree Widget:
-      Expanded:
-        - /Global Options1
-        - /Status1
-        - /RobotModel1
-        - /TF1
-        - /TF1/Frames1
-      Splitter Ratio: 0.5
-    Tree Height: 617
-  - Class: rviz_common/Selection
-    Name: Selection
-  - Class: rviz_common/Tool Properties
-    Expanded:
-      - /2D Goal Pose1
-      - /Publish Point1
-    Name: Tool Properties
-    Splitter Ratio: 0.5886790156364441
-  - Class: rviz_common/Views
-    Expanded:
-      - /Current View1
-    Name: Views
-    Splitter Ratio: 0.5
-Visualization Manager:
-  Class: ""
-  Displays:
-    - Alpha: 0.5
-      Cell Size: 1
-      Class: rviz_default_plugins/Grid
-      Color: 160; 160; 164
-      Enabled: true
-      Line Style:
-        Line Width: 0.029999999329447746
-        Value: Lines
-      Name: Grid
-      Normal Cell Count: 0
-      Offset:
-        X: 0
-        Y: 0
-        Z: 0
-      Plane: XY
-      Plane Cell Count: 10
-      Reference Frame: <Fixed Frame>
-      Value: true
-    - Alpha: 1
-      Class: rviz_default_plugins/RobotModel
-      Collision Enabled: false
-      Description File: ""
-      Description Source: Topic
-      Description Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /robot_description
-      Enabled: true
-      Links:
-        All Links Enabled: true
-        Expand Joint Details: false
-        Expand Link Details: false
-        Expand Tree: false
-        Link Tree Style: Links in Alphabetic Order
-        base_footprint:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        drivewhl_l_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        drivewhl_r_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_caster:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        gps_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        imu_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        lidar_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-      Name: RobotModel
-      TF Prefix: ""
-      Update Interval: 0
-      Value: true
-      Visual Enabled: true
-    - Class: rviz_default_plugins/TF
-      Enabled: true
-      Frame Timeout: 15
-      Frames:
-        All Enabled: false
-        base_footprint:
-          Value: false
-        base_link:
-          Value: false
-        drivewhl_l_link:
-          Value: true
-        drivewhl_r_link:
-          Value: true
-        front_caster:
-          Value: false
-        gps_link:
-          Value: false
-        imu_link:
-          Value: false
-        lidar_link:
-          Value: false
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: false
-      Show Axes: true
-      Show Names: true
-      Tree:
-        base_footprint:
-          base_link:
-            drivewhl_l_link:
-              {}
-            drivewhl_r_link:
-              {}
-            front_caster:
-              {}
-            gps_link:
-              {}
-            imu_link:
-              {}
-            lidar_link:
-              {}
-      Update Interval: 0
-      Value: true
-  Enabled: true
-  Global Options:
-    Background Color: 48; 48; 48
-    Fixed Frame: base_link
-    Frame Rate: 30
-  Name: root
-  Tools:
-    - Class: rviz_default_plugins/Interact
-      Hide Inactive Objects: true
-    - Class: rviz_default_plugins/MoveCamera
-    - Class: rviz_default_plugins/Select
-    - Class: rviz_default_plugins/FocusCamera
-    - Class: rviz_default_plugins/Measure
-      Line color: 128; 128; 0
-    - Class: rviz_default_plugins/SetInitialPose
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /initialpose
-    - Class: rviz_default_plugins/SetGoal
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /goal_pose
-    - Class: rviz_default_plugins/PublishPoint
-      Single click: true
-      Topic:
-        Depth: 5
-        Durability Policy: Volatile
-        History Policy: Keep Last
-        Reliability Policy: Reliable
-        Value: /clicked_point
-  Transformation:
-    Current:
-      Class: rviz_default_plugins/TF
-  Value: true
-  Views:
-    Current:
-      Class: rviz_default_plugins/Orbit
-      Distance: 4.434264183044434
-      Enable Stereo Rendering:
-        Stereo Eye Separation: 0.05999999865889549
-        Stereo Focal Distance: 1
-        Swap Stereo Eyes: false
-        Value: false
-      Focal Point:
-        X: 0.31193429231643677
-        Y: 0.11948385089635849
-        Z: -0.4807402193546295
-      Focal Shape Fixed Size: true
-      Focal Shape Size: 0.05000000074505806
-      Invert Z Axis: false
-      Name: Current View
-      Near Clip Distance: 0.009999999776482582
-      Pitch: 0.490397572517395
-      Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 1.0503965616226196
-    Saved: ~
-Window Geometry:
-  Displays:
-    collapsed: false
-  Height: 846
-  Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
-  Selection:
-    collapsed: false
-  Tool Properties:
-    collapsed: false
-  Views:
-    collapsed: false
-  Width: 1200
-  X: 246
-  Y: 77
--- a/src/core_rover_description/export.log
+++ b/src/core_rover_description/export.log
--- a/src/core_rover_description/launch/display.launch.py
+++ b/src/core_rover_description/launch/display.launch.py
@@ -1,117 +0,0 @@
-# Author: Addison Sears-Collins
-# Date: September 14, 2021
-# Description: Launch a two-wheeled robot URDF file using Rviz.
-# https://automaticaddison.com
- 
-import os
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import Command, LaunchConfiguration
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-from launch_ros.parameter_descriptions import ParameterValue
- 
-def generate_launch_description():
- 
-  # Set the path to this package.
-  pkg_share = FindPackageShare(package='core_rover_description').find('core_rover_description')
- 
-  # Set the path to the RViz configuration settings
-  default_rviz_config_path = os.path.join(pkg_share, 'config/rviz_basic_settings.rviz')
- 
-  # Set the path to the URDF file
-  default_urdf_model_path = os.path.join(pkg_share, 'urdf/core_rover_description.urdf')
- 
-  ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############  
-  # Launch configuration variables specific to simulation
-  gui = LaunchConfiguration('gui')
-  urdf_model = LaunchConfiguration('urdf_model')
-  rviz_config_file = LaunchConfiguration('rviz_config_file')
-  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
-  use_rviz = LaunchConfiguration('use_rviz')
-  use_sim_time = LaunchConfiguration('use_sim_time')
- 
-  # Declare the launch arguments  
-  declare_urdf_model_path_cmd = DeclareLaunchArgument(
-    name='urdf_model', 
-    default_value=default_urdf_model_path, 
-    description='Absolute path to robot urdf file')
-     
-  declare_rviz_config_file_cmd = DeclareLaunchArgument(
-    name='rviz_config_file',
-    default_value=default_rviz_config_path,
-    description='Full path to the RVIZ config file to use')
-     
-  declare_use_joint_state_publisher_cmd = DeclareLaunchArgument(
-    name='gui',
-    default_value='True',
-    description='Flag to enable joint_state_publisher_gui')
-   
-  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
-    name='use_robot_state_pub',
-    default_value='True',
-    description='Whether to start the robot state publisher')
- 
-  declare_use_rviz_cmd = DeclareLaunchArgument(
-    name='use_rviz',
-    default_value='True',
-    description='Whether to start RVIZ')
-     
-  declare_use_sim_time_cmd = DeclareLaunchArgument(
-    name='use_sim_time',
-    default_value='True',
-    description='Use simulation (Gazebo) clock if true')
-    
-  # Specify the actions
- 
-  # Publish the joint state values for the non-fixed joints in the URDF file.
-  start_joint_state_publisher_cmd = Node(
-    condition=UnlessCondition(gui),
-    package='joint_state_publisher',
-    executable='joint_state_publisher',
-    name='joint_state_publisher')
- 
-  # A GUI to manipulate the joint state values
-  start_joint_state_publisher_gui_node = Node(
-    condition=IfCondition(gui),
-    package='joint_state_publisher_gui',
-    executable='joint_state_publisher_gui',
-    name='joint_state_publisher_gui')
- 
-  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
-  start_robot_state_publisher_cmd = Node(
-    condition=IfCondition(use_robot_state_pub),
-    package='robot_state_publisher',
-    executable='robot_state_publisher',
-    parameters=[{'use_sim_time': use_sim_time, 
-                 'robot_description': ParameterValue(Command(['xacro ', urdf_model]), value_type=str)}],
-    arguments=[default_urdf_model_path])
- 
-  # Launch RViz
-  start_rviz_cmd = Node(
-    condition=IfCondition(use_rviz),
-    package='rviz2',
-    executable='rviz2',
-    name='rviz2',
-    output='screen',
-    arguments=['-d', rviz_config_file])
-   
-  # Create the launch description and populate
-  ld = LaunchDescription()
- 
-  # Declare the launch options
-  ld.add_action(declare_urdf_model_path_cmd)
-  ld.add_action(declare_rviz_config_file_cmd)
-  ld.add_action(declare_use_joint_state_publisher_cmd)
-  ld.add_action(declare_use_robot_state_pub_cmd)  
-  ld.add_action(declare_use_rviz_cmd) 
-  ld.add_action(declare_use_sim_time_cmd)
- 
-  # Add any actions
-  ld.add_action(start_joint_state_publisher_cmd)
-  ld.add_action(start_joint_state_publisher_gui_node)
-  ld.add_action(start_robot_state_publisher_cmd)
-  ld.add_action(start_rviz_cmd)
- 
-  return ld
--- a/src/core_rover_description/launch/robot_state_publisher.launch.py
+++ b/src/core_rover_description/launch/robot_state_publisher.launch.py
@@ -1,238 +0,0 @@
-#!/usr/bin/env python3
-"""
-Launch RViz visualization for the mycobot robot.
-
-This launch file sets up the complete visualization environment for the mycobot robot,
-including robot state publisher, joint state publisher, and RViz2. It handles loading
-and processing of URDF/XACRO files and controller configurations.
-
-:author: Addison Sears-Collins
-:date: November 15, 2024
-"""
-import os
-from pathlib import Path
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
-from launch.conditions import IfCondition, UnlessCondition
-from launch.substitutions import Command, LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterValue
-from launch_ros.substitutions import FindPackageShare
-
-
-def process_ros2_controllers_config(context):
-    """Process the ROS 2 controller configuration yaml file before loading the URDF.
-
-    This function reads a template configuration file, replaces placeholder values
-    with actual configuration, and writes the processed file to both source and
-    install directories.
-
-    Args:
-        context: Launch context containing configuration values
-
-    Returns:
-        list: Empty list as required by OpaqueFunction
-    """
-
-    # Get the configuration values
-    prefix = LaunchConfiguration('prefix').perform(context)
-    flange_link = LaunchConfiguration('flange_link').perform(context)
-    robot_name = LaunchConfiguration('robot_name').perform(context)
-
-    home = str(Path.home())
-
-    # Define both source and install paths
-    src_config_path = os.path.join(
-        home,
-        'ros2_ws/src/mycobot_ros2/mycobot_moveit_config/config',
-        robot_name
-    )
-    install_config_path = os.path.join(
-        home,
-        'ros2_ws/install/mycobot_moveit_config/share/mycobot_moveit_config/config',
-        robot_name
-    )
-
-    # Read from source template
-    template_path = os.path.join(src_config_path, 'ros2_controllers_template.yaml')
-    with open(template_path, 'r', encoding='utf-8') as file:
-        template_content = file.read()
-
-    # Create processed content (leaving template untouched)
-    processed_content = template_content.replace('${prefix}', prefix)
-    processed_content = processed_content.replace('${flange_link}', flange_link)
-
-    # Write processed content to both source and install directories
-    for config_path in [src_config_path, install_config_path]:
-        os.makedirs(config_path, exist_ok=True)
-        output_path = os.path.join(config_path, 'ros2_controllers.yaml')
-        with open(output_path, 'w', encoding='utf-8') as file:
-            file.write(processed_content)
-
-    return []
-
-
-# Define the arguments for the XACRO file
-ARGUMENTS = [
-    DeclareLaunchArgument('robot_name', default_value='mycobot_280',
-                          description='Name of the robot'),
-    DeclareLaunchArgument('prefix', default_value='',
-                          description='Prefix for robot joints and links'),
-    DeclareLaunchArgument('add_world', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to add world link'),
-    DeclareLaunchArgument('base_link', default_value='base_link',
-                          description='Name of the base link'),
-    DeclareLaunchArgument('base_type', default_value='g_shape',
-                          description='Type of the base'),
-    DeclareLaunchArgument('flange_link', default_value='link6_flange',
-                          description='Name of the flange link'),
-    DeclareLaunchArgument('gripper_type', default_value='adaptive_gripper',
-                          description='Type of the gripper'),
-    DeclareLaunchArgument('use_camera', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use the RGBD Gazebo plugin for point cloud'),
-    DeclareLaunchArgument('use_gazebo', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use Gazebo simulation'),
-    DeclareLaunchArgument('use_gripper', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to attach a gripper')
-]
-
-
-def generate_launch_description():
-    """Generate the launch description for the mycobot robot visualization.
-
-    This function sets up all necessary nodes and parameters for visualizing
-    the mycobot robot in RViz, including:
-    - Robot state publisher for broadcasting transforms
-    - Joint state publisher for simulating joint movements
-    - RViz for visualization
-
-    Returns:
-        LaunchDescription: Complete launch description for the visualization setup
-    """
-    # Define filenames
-    urdf_package = 'core_rover_description'
-    urdf_filename = 'core_rover_description.urdf'
-    rviz_config_filename = 'rviz_basic_settings.rviz'
-
-    # Set paths to important files
-    pkg_share_description = FindPackageShare(urdf_package)
-    default_urdf_model_path = PathJoinSubstitution(
-        [pkg_share_description, 'urdf', urdf_filename])
-    default_rviz_config_path = PathJoinSubstitution(
-        [pkg_share_description, 'rviz', rviz_config_filename])
-
-    # Launch configuration variables
-    jsp_gui = LaunchConfiguration('jsp_gui')
-    rviz_config_file = LaunchConfiguration('rviz_config_file')
-    urdf_model = LaunchConfiguration('urdf_model')
-    use_jsp = LaunchConfiguration('use_jsp')
-    use_rviz = LaunchConfiguration('use_rviz')
-    use_sim_time = LaunchConfiguration('use_sim_time')
-
-    # Declare the launch arguments
-    declare_jsp_gui_cmd = DeclareLaunchArgument(
-        name='jsp_gui',
-        default_value='true',
-        choices=['true', 'false'],
-        description='Flag to enable joint_state_publisher_gui')
-
-    declare_rviz_config_file_cmd = DeclareLaunchArgument(
-        name='rviz_config_file',
-        default_value=default_rviz_config_path,
-        description='Full path to the RVIZ config file to use')
-
-    declare_urdf_model_path_cmd = DeclareLaunchArgument(
-        name='urdf_model',
-        default_value=default_urdf_model_path,
-        description='Absolute path to robot urdf file')
-
-    declare_use_jsp_cmd = DeclareLaunchArgument(
-        name='use_jsp',
-        default_value='false',
-        choices=['true', 'false'],
-        description='Enable the joint state publisher')
-
-    declare_use_rviz_cmd = DeclareLaunchArgument(
-        name='use_rviz',
-        default_value='true',
-        description='Whether to start RVIZ')
-
-    declare_use_sim_time_cmd = DeclareLaunchArgument(
-        name='use_sim_time',
-        default_value='false',
-        description='Use simulation (Gazebo) clock if true')
-
-    robot_description_content = ParameterValue(Command([
-        'xacro', ' ', urdf_model, ' ',
-        'robot_name:=', LaunchConfiguration('robot_name'), ' ',
-        'prefix:=', LaunchConfiguration('prefix'), ' ',
-        'add_world:=', LaunchConfiguration('add_world'), ' ',
-        'base_link:=', LaunchConfiguration('base_link'), ' ',
-        'base_type:=', LaunchConfiguration('base_type'), ' ',
-        'flange_link:=', LaunchConfiguration('flange_link'), ' ',
-        'gripper_type:=', LaunchConfiguration('gripper_type'), ' ',
-        'use_camera:=', LaunchConfiguration('use_camera'), ' ',
-        'use_gazebo:=', LaunchConfiguration('use_gazebo'), ' ',
-        'use_gripper:=', LaunchConfiguration('use_gripper')
-    ]), value_type=str)
-
-    # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
-    start_robot_state_publisher_cmd = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        output='screen',
-        parameters=[{
-            'use_sim_time': use_sim_time,
-            'robot_description': robot_description_content}])
-
-    # Publish the joint state values for the non-fixed joints in the URDF file.
-    start_joint_state_publisher_cmd = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(use_jsp))
-
-    # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
-    start_joint_state_publisher_gui_cmd = Node(
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(jsp_gui))
-
-    # Launch RViz
-    start_rviz_cmd = Node(
-        condition=IfCondition(use_rviz),
-        package='rviz2',
-        executable='rviz2',
-        output='screen',
-        arguments=['-d', rviz_config_file],
-        parameters=[{'use_sim_time': use_sim_time}])
-
-    # Create the launch description and populate
-    ld = LaunchDescription(ARGUMENTS)
-
-    # Process the controller configuration before starting nodes
-    # ld.add_action(OpaqueFunction(function=process_ros2_controllers_config))
-
-    # Declare the launch options
-    ld.add_action(declare_jsp_gui_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
-    ld.add_action(declare_urdf_model_path_cmd)
-    ld.add_action(declare_use_jsp_cmd) 
-    ld.add_action(declare_use_rviz_cmd)
-    ld.add_action(declare_use_sim_time_cmd)
-
-    # Add any actions
-    ld.add_action(start_joint_state_publisher_cmd)
-    ld.add_action(start_joint_state_publisher_gui_cmd)
-    ld.add_action(start_robot_state_publisher_cmd)
-    ld.add_action(start_rviz_cmd)
-
-    return ld
--- a/src/core_rover_description/meshes/averaging_bar.STL
+++ b/src/core_rover_description/meshes/averaging_bar.STL
--- a/src/core_rover_description/meshes/base_link.STL
+++ b/src/core_rover_description/meshes/base_link.STL
--- a/src/core_rover_description/meshes/bl_wheel.STL
+++ b/src/core_rover_description/meshes/bl_wheel.STL
--- a/src/core_rover_description/meshes/br_wheel.STL
+++ b/src/core_rover_description/meshes/br_wheel.STL
--- a/src/core_rover_description/meshes/fl_wheel.STL
+++ b/src/core_rover_description/meshes/fl_wheel.STL
--- a/src/core_rover_description/meshes/fr_wheel.STL
+++ b/src/core_rover_description/meshes/fr_wheel.STL
--- a/src/core_rover_description/meshes/left_suspension_member.STL
+++ b/src/core_rover_description/meshes/left_suspension_member.STL
--- a/src/core_rover_description/meshes/right_suspension_member.STL
+++ b/src/core_rover_description/meshes/right_suspension_member.STL
--- a/src/core_rover_description/package.xml
+++ b/src/core_rover_description/package.xml
@@ -1,17 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>core_rover_description</name>
-  <version>1.0.0</version>
-  <description>This package contains configuration data, 3D models and launch files for Clucky Core</description>
-  <maintainer email="ds0196@uah.edu">David Sharpe</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <depend>rclpy</depend>
-  <depend>robot_state_publisher</depend>
-  <depend>rviz2</depend>
-
-  <export>
-    <build_type>ament_python</build_type>
-  </export>
-</package>
--- a/src/core_rover_description/resource/core_rover_description
+++ b/src/core_rover_description/resource/core_rover_description
--- a/src/core_rover_description/setup.cfg
+++ b/src/core_rover_description/setup.cfg
@@ -1,4 +0,0 @@
-[develop]
-script_dir=$base/lib/core_ik_pkg
-[install]
-install_scripts=$base/lib/core_ik_pkg
--- a/src/core_rover_description/setup.py
+++ b/src/core_rover_description/setup.py
@@ -1,28 +0,0 @@
-from setuptools import find_packages, setup
-import os
-from glob import glob
-
-package_name = 'core_rover_description'
-
-list_of_folders = ["config", "launch", "meshes", "textures", "urdf"]
-
-setup_data_files = [
-    ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-    ('share/' + package_name, ['package.xml'])
-]
-
-for folder in list_of_folders:
-    setup_data_files.append((os.path.join('share', package_name, folder), glob(folder + '/*')))
-
-setup(
-    name=package_name,
-    version='1.0.0',
-    packages=find_packages(exclude=['test']),
-    data_files=setup_data_files,  # created above with for loop
-    install_requires=['setuptools'],
-    zip_safe=True,
-    maintainer='David Sharpe',
-    maintainer_email='ds0196@uah.edu',
-    description='This package contains configuration data, 3D models and launch files for Clucky Core',
-    license='BSD'
-)
--- a/src/core_rover_description/urdf/core_rover_description.csv
+++ b/src/core_rover_description/urdf/core_rover_description.csv
@@ -1,9 +0,0 @@
-Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
-base_link,-6.65913085716885E-06,0.109571969438869,-0.0865730790907174,0,0,0,75.3855391061812,2.40019927956544,0.000116237424420155,-0.00156114013755156,2.05138658583389,0.00559155518698155,4.26537268155378,0,0,0,0,0,0,package://core_rover_description/meshes/base_link.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/base_link.STL,,Shell & Averaging System-1/CSC Shell Assembly-1/CSC V4 Shell-1;Motor and 6 pin connector mount-2;Motor and 6 pin connector mount-1;Inside Motor and 6 pin connector mount plate-1;Shell & Averaging System-1/Diff Box Backing Plate V2-1;Shell & Averaging System-1/Averaging System Outer Plate Backing V1-1;Shell & Averaging System-1/Arm Bracket Assembly Right V3-1/L bracket-1;Shell & Averaging System-1/MirrorArm Bracket Assembly-2/MirrorArm Bolts-1;Shell & Averaging System-1/Arm Bracket Assembly Right V3-1/Arm Bolts-1;Elec_Box_Assem-1/Box_Base-1;Elec_Box_Assem-1/Box_Left_Wall-1;Elec_Box_Assem-1/Box_Front_Wall-1;Elec_Box_Assem-1/Box_Right_Wall-1;Elec_Box_Assem-1/POE Switch (Measured with hole pattern)-1;Elec_Box_Assem-1/Ethernet Switch (Measured with hole pattern)-1;Elec_Box_Assem-1/NUC13ANH_NUC13LCH-1/NUC13ANH_NUC13LCH.STP-1/00_WS_ALL_ASM_CHECK_ASM_1_ASM_1.STP-1/WS_ME_PLACEMENT_ASM_ASM_1_ASM_2.STP-1/WS_ME_TALL_ASSY_ASM_1_ASM_1.STP-1/WS_TOP_COVER_ASM_ASM_1_ASM_1.STP-1/AN_TOP_COVER_1_2.STP-1;Elec_Box_Assem-1/NUC13ANH_NUC13LCH-1/NUC13ANH_NUC13LCH.STP-1/00_WS_ALL_ASM_CHECK_ASM_1_ASM_1.STP-1/WS_ME_PLACEMENT_ASM_ASM_1_ASM_2.STP-1/WS_ME_TALL_ASSY_ASM_1_ASM_1.STP-1/WS_TALL_MID_FRAME_ASM_ASM_1_ASM_1.STP-1/WS_TALL_MID_FRAME_NEW_1_1.STP-1;Elec_Box_Assem-1/SolidStateRelay-1;Elec_Box_Assem-1/100AFuse-1;Elec_Box_Assem-1/Box_Back_Wall-1;Rear_Wall_Sheath_Outer-2;Rear_Wall_Sheath_Outer-1;Shell & Averaging System-1/MirrorArm Bracket Assembly-2/MirrorL bracket-1;Shell & Averaging System-1/Chasis C-Channel Lip-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Core Mount Front Plate-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Core Mount Front Plate-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-9;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-10;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Easy To weld Tube Spacer-8;Shell & Averaging System-1/Averaging System Outer Plate V3-1;Shell & Averaging System-1/Diff V5 Assem-2/Diff Box V5-1;Shell & Averaging System-1/Averaging System Outer Plate V3-2;Shell & Averaging System-1/picatinny95mm-1;Shell & Averaging System-1/picatinny95mm-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-3;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-4;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/90044A425_Black-Oxide Alloy Steel Socket Head Screw-2;Shell & Averaging System-1/6436K14 .5 Shaft Collar-2;Shell & Averaging System-1/6436K14 .5 Shaft Collar-4;Shell & Averaging System-1/6436K14 .5 Shaft Collar-3;Shell & Averaging System-1/6436K14 .5 Shaft Collar-1;Rear_Wall_Sheath_Inner-1;Shell & Averaging System-1/8896T69 SS U Bolt 1.375in-1;Shell & Averaging System-1/8896T69 SS U Bolt 1.375in-2;Shell & Averaging System-1/Averaging Shaft V3-2;Shell & Averaging System-1/Averaging Shaft V3-3;Shell & Averaging System-1/Diff V5 Assem-2/2342K186_Bearing .5 shaft sealed-3;Shell & Averaging System-1/2342K186_Bearing .5 shaft sealed-1;Shell & Averaging System-1/2342K186_Bearing .5 shaft sealed-2,Origin_global,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
-right_suspension_member,0.0011302,-0.12483,0.029359,0,0,0,1.7681,0.0021448,-1.4957E-06,-2.5225E-08,0.0068473,0.00021191,0.0058417,0,0,0,0,0,0,package://core_rover_description/meshes/right_suspension_member.STL,1,1,1,1,0,0,0,0,0,0,package://core_rover_description/meshes/right_suspension_member.STL,,A- POST BOND SUSPENSION (1)-4/P- CENTER LEG [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- CORE ATTACHMENT [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- AXIS PLATE [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-4/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-2;A- POST BOND SUSPENSION (1)-4/P- SIDE LEGS [POST BOND SUSPENSION] (1)-2;Gearbox-Wheel Assembly - MIRROR-6/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-6/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-6/Bonding Tube-1;Gearbox-Wheel Assembly - MIRROR-6/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-6/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/Gearbox Casing-1;A- POST BOND SUSPENSION (1)-4/P- SIDE LEGS [POST BOND SUSPENSION] (1)-1;Gearbox-Wheel Assembly - MIRROR-7/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-7/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-7/Bonding Tube-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube 2-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/94645A111_High-Strength Steel Nylon-Insert Locknut-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/96144A239_Fine-Thread Alloy Steel Socket Head Screw-1,Origin_right_suspension_manual,right_suspension_axis,right_suspension_joint,revolute,-0.33067,0.028531,-0.15443,1.5708,0,-1.5708,base_link,0,0,-1,0,0,-0.38,0.38,,,,,,,,
-br_wheel,-0.0476013497459569,-1.09801088221673E-08,6.12845485470359E-09,0,0,0,5.74175792756056,0.115468209215025,7.19767266741869E-10,-2.90603405999934E-10,0.0647068246341468,7.16669744358495E-07,0.0632464587169257,0,0,0,0,0,0,package://core_rover_description/meshes/br_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/br_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-7/Updated Wheel Starboard-1,Origin_br_wheel_axis,br_wheel_axis,br_wheel_axis,continuous,-0.41838,-0.17155,0.12984,0.52965,-1.5708,0,right_suspension_member,1,0,0,,,,,,,,,,,,
-fr_wheel,-0.0476013497450763,-1.09822306249008E-08,6.12913009234717E-09,0,0,0,5.74175792767292,0.115468209216444,7.20424942594475E-10,-2.90800382125705E-10,0.0647068246345494,7.16670201096685E-07,0.063246458718483,0,0,0,0,0,0,package://core_rover_description/meshes/fr_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/fr_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-6/Updated Wheel Starboard-1,Origin_fr_wheel_joint,fr_wheel_axis,fr_wheel_joint,continuous,0.41838,-0.17155,0.12984,1.5769,-1.5708,0,right_suspension_member,1,0,0,,,,,,,,,,,,
-averaging_bar,5.12266309124314E-10,0.0103345512865068,6.28647148226413E-10,0,0,0,0.872230516841863,0.000102234478668288,1.26173058927933E-11,-1.00509715989169E-11,0.0236921798518964,1.54032135551904E-11,0.0236577000709469,0,0,0,0,0,0,package://core_rover_description/meshes/averaging_bar.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/averaging_bar.STL,,A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/Old Averaging Bar (Modified)-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/92981A220_Alloy Steel Shoulder Screws-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/92981A220_Alloy Steel Shoulder Screws-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube-1,Origin_averaging_bar_axis,averaging_bar_axis,averaging_bar_axis,revolute,0,-0.23362,-0.0508,0,-0.0060774,-3.1416,base_link,0,-1,0,0,0,-0.38,0.38,,,,,,,,
-left_suspension_member,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,package://core_rover_description/meshes/left_suspension_member.STL,1,1,1,1,0,0,0,0,0,0,package://core_rover_description/meshes/left_suspension_member.STL,,A- POST BOND SUSPENSION (1)-3/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- CENTER LEG [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- LEG ATTACHMENT CF COVER [POST BOND SUSPENSION] (1)-2;A- POST BOND SUSPENSION (1)-3/P- SIDE LEGS [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- SIDE LEGS [POST BOND SUSPENSION] (1)-2;Gearbox-Wheel Assembly - MIRROR-2/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-2/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-2/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/Gearbox Casing-1;Gearbox-Wheel Assembly - MIRROR-1/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-XXXX - 57 Sport 3 Stage Ring Gear Housing.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/3 Stage HD - 57 Sport-1/3 Stage HD - 57 Sport.STEP-1/am-3765 - 57 Sport Motor Block.STEP-1;Gearbox-Wheel Assembly - MIRROR-1/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-1/Bonding Tube-1;Gearbox-Wheel Assembly - MIRROR-2/REV-21-1651.step-1;Gearbox-Wheel Assembly - MIRROR-2/Bonding Tube-1;A- POST BOND SUSPENSION (1)-3/P- CORE ATTACHMENT [POST BOND SUSPENSION] (1)-1;A- POST BOND SUSPENSION (1)-3/P- AXIS PLATE [POST BOND SUSPENSION] (1)-1;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/6045N122_Low-Carbon Steel Round Tube 2-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/96144A239_Fine-Thread Alloy Steel Socket Head Screw-2;A - AVERAGING BAR ASSEMBLY [CORE ROVER WITH PBS] (1)-1/94645A111_High-Strength Steel Nylon-Insert Locknut-2,Origin_left_suspension_manual,left_suspension_axis,left_suspension_joint,revolute,0.33067,0.028531,-0.15443,1.5708,0,1.5708,base_link,0,0,-1,0,0,-0.38,0.38,,,,,,,,
-bl_wheel,-0.0476013497470983,-1.09827822947217E-08,6.12940226352165E-09,0,0,0,5.7417579274106,0.115468209213072,7.20585760708907E-10,-2.90859312873504E-10,0.0647068246335877,7.16669141063303E-07,0.0632464587148044,0,0,0,0,0,0,package://core_rover_description/meshes/bl_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/bl_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-1/Updated Wheel Starboard-1,Origin_bl_wheel_joint,bl_wheel_axis,bl_wheel_joint,continuous,0.41838,-0.17155,0.12984,1.5769,-1.5708,0,left_suspension_member,1,0,0,,,,,,,,,,,,
-fl_wheel,-0.0476013497448114,-1.09827921201955E-08,6.12933048760311E-09,0,0,0,5.74175792770551,0.115468209216877,7.20541990109492E-10,-2.90874836866816E-10,0.0647068246346634,7.16670369639572E-07,0.06324645871897,0,0,0,0,0,0,package://core_rover_description/meshes/fl_wheel.STL,0.792156862745098,0.819607843137255,0.933333333333333,1,0,0,0,0,0,0,package://core_rover_description/meshes/fl_wheel.STL,,Gearbox-Wheel Assembly - MIRROR-2/Updated Wheel Starboard-1,Origin_fl_wheel_joint,fl_wheel_axis,fl_wheel_joint,continuous,-0.418377874454344,-0.171553199039643,0.129841324042317,0.529654981264331,-1.5707963267949,0,left_suspension_member,1,0,0,,,,,,,,,,,,
--- a/src/core_rover_description/urdf/core_rover_description.urdf
+++ b/src/core_rover_description/urdf/core_rover_description.urdf
@@ -1,457 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version:   Build Version: 1.6.9403.28032
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot
-  name="core_rover_description">
-
-  <link
-    name="base_link">
-    <inertial>
-      <origin
-        xyz="-6.65913085716885E-06 0.109571969438869 -0.0865730790907174"
-        rpy="0 0 0" />
-      <mass
-        value="75.3855391061812" />
-      <inertia
-        ixx="2.40019927956544"
-        ixy="0.000116237424420155"
-        ixz="-0.00156114013755156"
-        iyy="2.05138658583389"
-        iyz="0.00559155518698155"
-        izz="4.26537268155378" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/base_link.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/base_link.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <link
-    name="right_suspension_member">
-    <inertial>
-      <origin
-        xyz="0.0011302 -0.12483 0.029359"
-        rpy="0 0 0" />
-      <mass
-        value="1.7681" />
-      <inertia
-        ixx="0.0021448"
-        ixy="-1.4957E-06"
-        ixz="-2.5225E-08"
-        iyy="0.0068473"
-        iyz="0.00021191"
-        izz="0.0058417" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/right_suspension_member.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.35 0.35 0.35 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/right_suspension_member.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="right_suspension_joint"
-    type="revolute">
-    <origin
-      xyz="-0.33067 0.028531 -0.15443"
-      rpy="1.5708 0 -1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="right_suspension_member" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-    <mimic
-      joint="left_suspension_joint"
-      multiplier="1"
-      offset="0" />
-  </joint>
-
-  <link
-    name="br_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497459569 -1.09801088221673E-08 6.12845485470359E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792756056" />
-      <inertia
-        ixx="0.115468209215025"
-        ixy="7.19767266741869E-10"
-        ixz="-2.90603405999934E-10"
-        iyy="0.0647068246341468"
-        iyz="7.16669744358495E-07"
-        izz="0.0632464587169257" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/br_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/br_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="br_wheel_axis"
-    type="continuous">
-    <origin
-      xyz="-0.41838 -0.17155 0.12984"
-      rpy="0.52965 -1.5708 0" />
-    <parent
-      link="right_suspension_member" />
-    <child
-      link="br_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="fr_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497450763 -1.09822306249008E-08 6.12913009234717E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792767292" />
-      <inertia
-        ixx="0.115468209216444"
-        ixy="7.20424942594475E-10"
-        ixz="-2.90800382125705E-10"
-        iyy="0.0647068246345494"
-        iyz="7.16670201096685E-07"
-        izz="0.063246458718483" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fr_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fr_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="fr_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="0.41838 -0.17155 0.12984"
-      rpy="1.5769 -1.5708 0" />
-    <parent
-      link="right_suspension_member" />
-    <child
-      link="fr_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="averaging_bar">
-    <inertial>
-      <origin
-        xyz="5.12266309124314E-10 0.0103345512865068 6.28647148226413E-10"
-        rpy="0 0 0" />
-      <mass
-        value="0.872230516841863" />
-      <inertia
-        ixx="0.000102234478668288"
-        ixy="1.26173058927933E-11"
-        ixz="-1.00509715989169E-11"
-        iyy="0.0236921798518964"
-        iyz="1.54032135551904E-11"
-        izz="0.0236577000709469" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/averaging_bar.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/averaging_bar.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="averaging_bar_axis"
-    type="revolute">
-    <origin
-      xyz="0 -0.23362 -0.0508"
-      rpy="0 -0.0060774 -3.1416" />
-    <parent
-      link="base_link" />
-    <child
-      link="averaging_bar" />
-    <axis
-      xyz="0 -1 0" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-    <mimic
-      joint="left_suspension_joint"
-      multiplier="-1"
-      offset="0" />
-  </joint>
-
-  <link
-    name="left_suspension_member">
-    <inertial>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <mass
-        value="1.7681" />
-      <inertia
-        ixx="0"
-        ixy="0"
-        ixz="0"
-        iyy="0"
-        iyz="0"
-        izz="0" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/left_suspension_member.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.35 0.35 0.35 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/left_suspension_member.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="left_suspension_joint"
-    type="revolute">
-    <origin
-      xyz="0.33067 0.028531 -0.15443"
-      rpy="1.5708 0 1.5708" />
-    <parent
-      link="base_link" />
-    <child
-      link="left_suspension_member" />
-    <axis
-      xyz="0 0 -1" />
-    <limit
-      lower="-0.38"
-      upper="0.38"
-      effort="0"
-      velocity="0" />
-  </joint>
-
-  <link
-    name="bl_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497470983 -1.09827822947217E-08 6.12940226352165E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.7417579274106" />
-      <inertia
-        ixx="0.115468209213072"
-        ixy="7.20585760708907E-10"
-        ixz="-2.90859312873504E-10"
-        iyy="0.0647068246335877"
-        iyz="7.16669141063303E-07"
-        izz="0.0632464587148044" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/bl_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/bl_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="bl_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="0.41838 -0.17155 0.12984"
-      rpy="1.5769 -1.5708 0" />
-    <parent
-      link="left_suspension_member" />
-    <child
-      link="bl_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-  <link
-    name="fl_wheel">
-    <inertial>
-      <origin
-        xyz="-0.0476013497448114 -1.09827921201955E-08 6.12933048760311E-09"
-        rpy="0 0 0" />
-      <mass
-        value="5.74175792770551" />
-      <inertia
-        ixx="0.115468209216877"
-        ixy="7.20541990109492E-10"
-        ixz="-2.90874836866816E-10"
-        iyy="0.0647068246346634"
-        iyz="7.16670369639572E-07"
-        izz="0.06324645871897" />
-    </inertial>
-    <visual>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fl_wheel.STL" />
-      </geometry>
-      <material
-        name="">
-        <color
-          rgba="0.01764195448412081 0.013702083043526807 0.030713443727452196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin
-        xyz="0 0 0"
-        rpy="0 0 0" />
-      <geometry>
-        <mesh
-          filename="package://core_rover_description/meshes/fl_wheel.STL" />
-      </geometry>
-    </collision>
-  </link>
-
-  <joint
-    name="fl_wheel_joint"
-    type="continuous">
-    <origin
-      xyz="-0.418377874454344 -0.171553199039643 0.129841324042317"
-      rpy="0.529654981264331 -1.5707963267949 0" />
-    <parent
-      link="left_suspension_member" />
-    <child
-      link="fl_wheel" />
-    <axis
-      xyz="1 0 0" />
-  </joint>
-
-</robot>
--- a/src/headless_pkg/package.xml
+++ b/src/headless_pkg/package.xml
@@ -8,7 +8,9 @@
  <license>AGPL-3.0-only</license>

  <depend>rclpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-pygame</depend>
+  <depend>astra_msgs</depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
--- a/src/headless_pkg/setup.py
+++ b/src/headless_pkg/setup.py
@@ -1,23 +1,23 @@
 from setuptools import find_packages, setup

-package_name = 'headless_pkg'
+package_name = "headless_pkg"

 setup(
    name=package_name,
-    version='1.0.0',
-    packages=find_packages(exclude=['test']),
+    version="1.0.0",
+    packages=find_packages(exclude=["test"]),
    data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
+        ("share/" + package_name, ["package.xml"]),
    ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
    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',
+    maintainer="David Sharpe",
+    maintainer_email="ds0196@uah.edu",
+    description="Headless rover control package to handle command interpretation and embedded interfacing.",
+    license="All Rights Reserved",
    entry_points={
-        'console_scripts': [
+        "console_scripts": [
            "headless_full = src.headless_node:main",
        ],
    },
--- a/src/headless_pkg/src/headless_node.py
+++ b/src/headless_pkg/src/headless_node.py
@@ -7,27 +7,29 @@ import signal
 import time
 import atexit

-import serial
 import os
 import sys
 import threading
 import glob
+from math import copysign

 from std_msgs.msg import String
 from geometry_msgs.msg import Twist
-from ros2_interfaces_pkg.msg import CoreControl, ArmManual, BioControl
-from ros2_interfaces_pkg.msg import CoreCtrlState
+from astra_msgs.msg import CoreControl, ArmManual, BioControl
+from astra_msgs.msg import CoreCtrlState

 import pygame

 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()
+os.environ["SDL_AUDIODRIVER"] = (
+    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+)


 CORE_STOP_MSG = CoreControl()  # All zeros by default
 CORE_STOP_TWIST_MSG = Twist()  # "
-ARM_STOP_MSG = ArmManual()     # "
-BIO_STOP_MSG = BioControl()    # "
+ARM_STOP_MSG = ArmManual()  # "
+BIO_STOP_MSG = BioControl()  # "

 control_qos = qos.QoSProfile(
    history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -37,7 +39,7 @@ control_qos = qos.QoSProfile(
    deadline=Duration(seconds=1),
    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    liveliness_lease_duration=Duration(seconds=5),
 )

 CORE_MODE = "twist"  # "twist" or "duty"
@@ -48,6 +50,14 @@ class Headless(Node):
        # Initialize pygame first
        pygame.init()
        pygame.joystick.init()
+        super().__init__("headless")
+
+        # Wait for anchor to start
+        pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
+        while len(pub_info) == 0:
+            self.get_logger().info("Waiting for anchor to start...")
+            time.sleep(1.0)
+            pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")

        # Wait for a gamepad to be connected
        print("Waiting for gamepad connection...")
@@ -63,19 +73,20 @@ class Headless(Node):
        # Initialize the gamepad
        self.gamepad = pygame.joystick.Joystick(0)
        self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
+        print(f"Gamepad Found: {self.gamepad.get_name()}")

-        # Now initialize the ROS2 node
-        super().__init__("headless")
        self.create_timer(0.15, self.send_controls)

-        self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
-        self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2)
-        self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2)
-
-        self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos)
-        self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
+        self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
+        self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
+        self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)

+        self.core_twist_pub_ = self.create_publisher(
+            Twist, "/core/twist", qos_profile=control_qos
+        )
+        self.core_state_pub_ = self.create_publisher(
+            CoreCtrlState, "/core/control/state", qos_profile=control_qos
+        )

        self.ctrl_mode = "core"  # Start in core mode
        self.core_brake_mode = False
@@ -84,12 +95,11 @@ class Headless(Node):
        # Rumble when node is ready (returns False if rumble not supported)
        self.gamepad.rumble(0.7, 0.8, 150)

-
    def run(self):
        # This thread makes all the update processes run in the background
        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
        thread.start()
-        
+
        try:
            while rclpy.ok():
                self.send_controls()
@@ -98,12 +108,12 @@ class Headless(Node):
            sys.exit(0)

    def send_controls(self):
-        """ Read the gamepad state and publish control messages """
+        """Read the gamepad state and publish control messages"""
        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...")
@@ -116,7 +126,6 @@ class Headless(Node):
            pygame.quit()
            sys.exit(0)

-
        new_ctrl_mode = self.ctrl_mode  # if "" then inequality will always be true

        # Check for control mode change
@@ -131,7 +140,6 @@ class Headless(Node):
            self.ctrl_mode = new_ctrl_mode
            self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")

-
        # CORE
        if self.ctrl_mode == "core" and CORE_MODE == "duty":
            input = CoreControl()
@@ -142,7 +150,6 @@ class Headless(Node):
            right_stick_y = deadzone(self.gamepad.get_axis(4))
            right_trigger = deadzone(self.gamepad.get_axis(5))

-
            # Right wheels
            input.right_stick = float(round(-1 * right_stick_y, 2))

@@ -152,15 +159,14 @@ class Headless(Node):
            else:
                input.left_stick = float(round(-1 * left_stick_y, 2))

-
            # Debug
-            output = f'L: {input.left_stick}, R: {input.right_stick}'
+            output = f"L: {input.left_stick}, R: {input.right_stick}"
            self.get_logger().info(f"[Ctrl] {output}")

            self.core_publisher.publish(input)
            self.arm_publisher.publish(ARM_STOP_MSG)
            # self.bio_publisher.publish(BIO_STOP_MSG)
-        
+
        elif self.ctrl_mode == "core" and CORE_MODE == "twist":
            input = Twist()

@@ -173,13 +179,17 @@ class Headless(Node):

            # Forward/back and Turn
            input.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * right_stick_x ** 2  # Exponent for finer control (curve)
+            input.angular.z = -1.0 * copysign(
+                right_stick_x**2, right_stick_x
+            )  # Exponent for finer control (curve)

            # Publish
            self.core_twist_pub_.publish(input)
            self.arm_publisher.publish(ARM_STOP_MSG)
            # self.bio_publisher.publish(BIO_STOP_MSG)
-            self.get_logger().info(f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}")
+            self.get_logger().info(
+                f"[Core Ctrl] Linear: {round(input.linear.x, 2)}, Angular: {round(input.angular.z, 2)}"
+            )

            # Brake mode
            new_brake_mode = button_a
@@ -192,20 +202,24 @@ class Headless(Node):
                new_max_duty = 0.5

            # Only publish if needed
-            if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
+            if (
+                new_brake_mode != self.core_brake_mode
+                or new_max_duty != self.core_max_duty
+            ):
                self.core_brake_mode = new_brake_mode
                self.core_max_duty = new_max_duty
                state_msg = CoreCtrlState()
                state_msg.brake_mode = bool(self.core_brake_mode)
                state_msg.max_duty = float(self.core_max_duty)
                self.core_state_pub_.publish(state_msg)
-                self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
-
+                self.get_logger().info(
+                    f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
+                )

        # ARM and BIO
        if self.ctrl_mode == "arm":
            arm_input = ArmManual()
-            
+
            # Collect controller state
            left_stick_x = deadzone(self.gamepad.get_axis(0))
            left_stick_y = deadzone(self.gamepad.get_axis(1))
@@ -216,7 +230,6 @@ class Headless(Node):
            right_bumper = self.gamepad.get_button(5)
            dpad_input = self.gamepad.get_hat(0)

-
            # EF Grippers
            if left_trigger > 0 and right_trigger > 0:
                arm_input.gripper = 0
@@ -231,7 +244,6 @@ class Headless(Node):
            elif dpad_input[0] == -1:
                arm_input.axis0 = -1

-
            if right_bumper:  # Control end effector

                # Effector yaw
@@ -246,23 +258,31 @@ class Headless(Node):
                elif right_stick_x < 0:
                    arm_input.effector_roll = -1

-            else:   # Control arm axis
+            else:  # Control arm axis

                # Axis 1
-                if abs(left_stick_x) > .15:
+                if abs(left_stick_x) > 0.15:
                    arm_input.axis1 = round(left_stick_x)

                # Axis 2
-                if abs(left_stick_y) > .15:
+                if abs(left_stick_y) > 0.15:
                    arm_input.axis2 = -1 * round(left_stick_y)

                # Axis 3
-                if abs(right_stick_y) > .15:
+                if abs(right_stick_y) > 0.15:
                    arm_input.axis3 = -1 * round(right_stick_y)

-
            # BIO
-            bio_input = BioControl(bio_arm=int(left_stick_y * -100), drill_arm=int(round(right_stick_y) * -100))
+            bio_input = BioControl(
+                bio_arm=int(left_stick_y * -100),
+                drill_arm=int(round(right_stick_y) * -100),
+            )
+
+            # Drill motor (FAERIE)
+            if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
+                bio_input.drill = int(
+                    30 * (right_trigger - left_trigger)
+                )  # Max duty cycle 30%

            self.core_publisher.publish(CORE_STOP_MSG)
            self.arm_publisher.publish(arm_input)
@@ -270,7 +290,7 @@ class Headless(Node):


 def deadzone(value: float, threshold=0.05) -> float:
-    """ Apply a deadzone to a joystick input so the motors don't sound angry """
+    """Apply a deadzone to a joystick input so the motors don't sound angry"""
    if abs(value) < threshold:
        return 0
    return value
@@ -282,6 +302,9 @@ def main(args=None):
    rclpy.spin(node)
    rclpy.shutdown()

-if __name__ == '__main__':
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+
+if __name__ == "__main__":
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
    main()
--- a/src/latency_tester/CMakeLists.txt
+++ b/src/latency_tester/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.8)
+cmake_minimum_required(VERSION 3.22)
 project(latency_tester)

 # Default to C++14
--- a/src/latency_tester/package.xml
+++ b/src/latency_tester/package.xml
@@ -9,7 +9,7 @@

  <buildtool_depend>ament_cmake</buildtool_depend>
  <depend>rclcpp</depend>
-  <depend>std_msgs</depend>
+  <depend>common_interfaces</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>
--- a/src/moveit_servo/CMakeLists.txt
+++ b/src/moveit_servo/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(core_gazebo)
+project(moveit_servo)

 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
@@ -7,19 +7,9 @@ endif()

 # find dependencies
 find_package(ament_cmake REQUIRED)
-find_package(controller_manager REQUIRED)
-find_package(gripper_controllers REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(ros2_control REQUIRED)
-find_package(ros2_controllers REQUIRED)
-find_package(trajectory_msgs REQUIRED)
-find_package(xacro REQUIRED)
-
-# Copy necessary files to designated locations in the project
-install (
-  DIRECTORY config launch models worlds
-  DESTINATION share/${PROJECT_NAME}
-)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)

 if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
--- a/src/moveit_servo/config/kinematics.yaml
+++ b/src/moveit_servo/config/kinematics.yaml
@@ -0,0 +1,4 @@
+panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05
--- a/src/moveit_servo/launch/servo.launch.py
+++ b/src/moveit_servo/launch/servo.launch.py
@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("astra_descriptions")
+        .robot_description(file_path="config/robot.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_node",
+        name="servo_node",
+        parameters=[
+            "config/servo_parameters.yaml",
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([servo_node])
--- a/src/moveit_servo/package.xml
+++ b/src/moveit_servo/package.xml
@@ -1,11 +1,11 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>core_gazebo</name>
+  <name>moveit_servo</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
-  <maintainer email="ds0196@uah.edu">David</maintainer>
-  <license>AGPL-3.0-only</license>
+  <maintainer email="aaron.m.davis@comcast.net">aaron</maintainer>
+  <license>TODO: License declaration</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

--- a/src/ros2_interfaces_pkg
+++ b/src/ros2_interfaces_pkg
Author	SHA1	Message	Date
Aaron Davis	f1d3a6c0de	added ros-gz-sim and ros-gz-bridge to packages	2025-11-19 15:20:25 -06:00
Aaron Davis	9713303e35	added ROS2 and colcon shell completion	2025-11-13 17:46:20 -06:00
Aaron Davis	d6dc62ee0f	removed topic-based-ros2-control package	2025-11-13 17:43:54 -06:00
David Sharpe	4a49069c2a	Merge pull request #24 from SHC-ASTRA/astra-msgs switch to astra_msgs	2025-11-07 01:52:08 -06:00
ryleu	d093c0b725	Merge branch 'main' into astra-msgs	2025-11-07 00:52:51 -06:00
David Sharpe	5d5f864cd7	Merge pull request #19 from SHC-ASTRA/black reformat with black	2025-11-07 00:14:35 -06:00
ryleu	d7fd133586	updated to astra_msgs	2025-11-06 21:35:47 -06:00
ryleu	c107b82a8d	reformat with black	2025-11-06 19:10:21 -06:00
ryleu	b670bc2eda	update flake	2025-11-06 19:09:57 -06:00
Riley M.	9fea136575	Merge pull request #23 from SHC-ASTRA/script-qol refactor: make autostart scripts use relative paths	2025-11-06 19:07:14 -06:00
David	0fa2226529	docs: rewrite README with new template	2025-11-04 01:12:45 -06:00
David	3ebd2e29a3	refactor: polish auto start scripts Add set -e and [ -z $ ]	2025-11-03 23:25:28 -06:00
ASTRA-SHC	9516e53f68	fix: make autostart script SCRIPT_DIR more robust	2025-10-26 12:20:16 +00:00
David	18a7b7e2ab	docs: standardize README	2025-10-26 06:54:13 -05:00
David	e351e4c991	fix: make auto start scripts work when not in specific dir	2025-10-26 06:54:05 -05:00
David Sharpe	f735f7194e	Merge pull request #20 from SHC-ASTRA/new_ik Integrate Moveit2, remove ikpy	2025-10-25 11:49:43 -05:00
ryleu	90e2aa5070	update shebangs to work on nixos	2025-10-25 11:15:32 -05:00
David	611ac90f54	style: cleanup servo_arm_twist_pkg CMakeLists	2025-10-25 11:15:32 -05:00
David	01ea43968d	feat: add moveit packages to flake.nix	2025-10-25 11:15:32 -05:00
David	4ce183773d	feat: finish removing old ikpy-based IK	2025-10-25 11:15:32 -05:00
David	3413615461	chore: remove astra_descriptions packages directly in src/	2025-10-25 11:15:31 -05:00
ryleu	9125391de9	remove ikpy (and reformat the code files)	2025-10-25 11:15:31 -05:00
David	981b0b166c	style: rename arm urdf packages * rover_urdf_pkg -> arm_description * astra_arm_moveit_config -> arm_moveit_config	2025-10-25 11:15:31 -05:00
David	9471992d3b	feat: add controller support	2025-10-25 11:15:31 -05:00
David Sharpe	9579b64cb0	refactor: remove arm_hardware_controller lmao Using premade topic based controller instead	2025-10-25 11:15:31 -05:00
David Sharpe	d92ca3ae5a	fix: remove spaces from link names to support Jammy Viz doesn't work when the links have spaces in their names on Jammy ._.	2025-10-25 11:15:31 -05:00
David Sharpe	d72a9a3b5e	feat: make Moveit2 demo talk to arm_pkg	2025-10-25 11:15:31 -05:00
David Sharpe	1b05202efa	feat: add arm_hardware_controller to act as a hardware interface for IK	2025-10-25 11:15:31 -05:00
David Sharpe	508fa8e2ae	fix: grippers now act correctly	2025-10-25 11:15:31 -05:00
David Sharpe	77bf59d5fd	fix: move roll joint to arm pose group, add real velocity limit to grippers	2025-10-25 11:15:31 -05:00
David Sharpe	0d09c81802	fix: remove space from joint name	2025-10-25 11:15:31 -05:00
David Sharpe	fa10027e2d	refactor: re-ran setup assistant	2025-10-25 11:15:31 -05:00
David Sharpe	bb2dda02a2	feat: add moveit2 configuration	2025-10-25 11:15:31 -05:00
David Sharpe	c0d39aa3a6	fix: make colcon build the new urdf package	2025-10-25 11:15:30 -05:00
David Sharpe	6671f290e5	feat: add new Arm URDF from SW in ROS1 package format	2025-10-25 11:15:30 -05:00
David Sharpe	2db9b67ebc	feat: add viz code from Tristan's ik_test and add CAD to URDF	2025-10-25 11:15:30 -05:00
David Sharpe	1281236b36	fix: I hate python (#21 ) BioControl.drill expects an int, have to explicitly cast to an int or headless will crash Makes main branch functional	2025-10-25 11:10:07 -05:00
Riley M.	281e5f39d3	Merge pull request #18 from SHC-ASTRA/ros2-control Flesh out ros2_control with Gazebo	2025-10-23 03:33:16 -05:00
David	fe1ae6120f	feat: all the features * Move rover-ros2/rover_launch.py to src/anchor_pkg/launch/, renamed to rover.launch.py * Anchor now waits to initialize topics until after it has found a microcontroller. * Headless now waits for anchor to start before it starts itself * Add default cases to motor feedback for motorId * Added black to the flake.nix and package.xml	2025-10-23 02:22:31 -05:00
David	44aa4b0848	feat: ramp drill speed	2025-10-23 01:00:13 -05:00
ryleu	c4f60d6814	clean up nitpicks	2025-10-23 00:52:19 -05:00
ryleu	87e3f06562	add direnv .envrc	2025-10-22 23:47:50 -05:00
David	cc53e6efd6	chore: update astra_descriptions	2025-10-22 23:40:04 -05:00
David	d879a3bae4	feat: add drill to bio headless	2025-10-21 10:41:34 -05:00
David	ed7efb4583	fix: ptz now works in flake	2025-10-18 12:28:53 -05:00
David	2165003f35	fix: msg len requirements now account for viccan packaging There are no viccan messages with len(data) == 3, only 1, 2, or 4	2025-10-18 12:17:45 -05:00
ryleu	95ceecacaa	do not build in shellHook	2025-10-18 02:39:08 -05:00
ryleu	414254b3b7	add a PORT_OVERRIDE env var	2025-10-18 02:35:09 -05:00
ryleu	a63a3b19af	fix python deps	2025-10-18 02:24:19 -05:00
David	b12515bf11	fix: rover can turn left again	2025-10-18 02:09:50 -05:00
David	8c01efeaf7	fix: make cmd_vel QoS compatible with everything else	2025-10-18 01:48:30 -05:00
David Sharpe	aa84667aab	feat: add depencies to packages.xml files, add packages to flake.nix	2025-10-17 18:06:54 -05:00
David	7ac250fd66	fix: correct descriptions submodule name	2025-10-16 18:05:13 -05:00
David	a7ec355c4f	style: move description files to new repo (astra_descriptions)	2025-10-16 18:03:43 -05:00
David	05af7f9be4	style: rename core_rover_description to core_description	2025-10-16 16:21:13 -05:00
David	5e8b60f720	style: move core_rover_description and core_gazebo into new astra_description folder	2025-10-16 12:33:10 -05:00
David	b9a63126e1	fix: correctly spawn controllers models was commented out in CMakeLists.txt because it is currently an empty directory, but will be used at a later point	2025-10-16 12:16:49 -05:00
David	a58f9b6ada	feat: make frames work properly, rviz is now accurate	2025-10-15 02:31:47 -05:00
David	89015ee7a5	feat: add VicCAN message validity checking and core feedback data length checking	2025-10-14 15:21:23 -05:00
David	d565dbe31f	refactor: change msg.command_id from if elif to match case	2025-10-14 13:49:53 -05:00
David	2d258b3103	refactor: new feedback topics use default sensor data QoS	2025-10-14 13:45:10 -05:00
David	86d01c29e3	fix: add gear ratio to JointState	2025-10-14 11:47:23 -05:00
David	366f1e0c58	feat: add joint_state pub to Core for wheel position and velocity	2025-10-13 21:50:09 -05:00
David	6bbb5d8706	refactor: make diff_controller listen on /core/rwist Now you can control it with headless or teleop_keyboard_twist	2025-10-10 23:37:19 -05:00
David	676f86bcd0	feat: make ros2 controllers start automatically Adds load_ros2_controllers.launch.py Weird issue where if the update rate on the controller is 10, then the controller manager doesn't respond to requests, but if you set it to 100 (still less than gazebo's rate), then it works...	2025-10-10 18:58:30 -05:00
David	723aa33e3c	fix: make work slightly more consistently	2025-10-10 14:16:47 -05:00
David	86684b0bff	feat: add ros2_control to Core URDF	2025-10-10 02:22:12 -05:00
				`@@ -1 +0,0 @@`
				`controller_joint_names: ['', 'right_suspension_joint', 'br_wheel_axis', 'fr_wheel_joint', 'averaging_bar_axis', 'left_suspension_joint', 'bl_wheel_joint', 'fl_wheel_joint', ]`