feat: (core) make compatible with ros2_control

Qos disable

.envrc

@@ -0,0 +1 @@
+use flake

.gitignore

@@ -10,4 +10,8 @@ log/
 .vscode/
 
 #Pycache folder
-__pycache__/
+__pycache__/
+
+#Direnv
+.direnv/
+.venv

.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

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` |

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
@@ -12,13 +15,14 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # 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

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

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
@@ -12,13 +15,14 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 
 # 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

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
@@ -13,9 +17,13 @@ done
 echo "[INFO] Network interface is up!"
 
 
-source /opt/ros/humble/setup.bash
+if command -v nixos-rebuild; then
+    echo "[INFO] running on NixOS"
+else
+    source /opt/ros/humble/setup.bash
+fi
 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
 

flake.lock

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

flake.nix

@@ -1,30 +1,91 @@
 {
+  description = "Development environment for ASTRA Anchor";
+
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs";  # IMPORTANT!!!
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    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 ];
         };
-      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.humble;
+              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-planners
+                  moveit-common
+                  moveit-msgs
+                  moveit-ros-planning
+                  moveit-ros-planning-interface
+		  moveit-ros-visualization
+                  moveit-configs-utils
+                  moveit-ros-move-group
+                  moveit-servo
+                  moveit-simple-controller-manager
+                  topic-based-ros2-control
+                  pilz-industrial-motion-planner
+                  pick-ik
+                  ompl
+                  joy
+                  ros2-controllers
+		  chomp-motion-planner
+                ];
+              }
+            )
           ];
+          shellHook = ''
+            # Display stuff
+            export DISPLAY=''${DISPLAY:-:0}
+            export QT_X11_NO_MITSHM=1
+          '';
         };
-      });
+      }
+    );
+
   nixConfig = {
     extra-substituters = [ "https://ros.cachix.org" ];
     extra-trusted-public-keys = [ "ros.cachix.org-1:dSyZxI8geDCJrwgvCOHDoAfOm5sV1wCPjBkKL+38Rvo=" ];

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)
-    ])

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from std_srvs.srv import Empty
 
 import signal
@@ -7,126 +8,222 @@ import time
 import atexit
 
 import serial
+import serial.tools.list_ports
 import os
 import sys
 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
+KNOWN_USBS = [
+    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
+    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
+    (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
+    (0x1A86, 0x55D3),  # ESP32 S3 Development Board
+]
 
 
-"""
-Publishers:
-  * /anchor/from_vic/debug
-    - Every string received from the MCU is published here for debugging
-  * /anchor/from_vic/core
-    - VicCAN messages for Core node
-  * /anchor/from_vic/arm
-    - VicCAN messages for Arm node
-  * /anchor/from_vic/bio
-    - VicCAN messages for Bio node
+class Anchor(Node):
+    """
+    Publishers:
+    * /anchor/from_vic/debug
+        - Every string received from the MCU is published here for debugging
+    * /anchor/from_vic/core
+        - VicCAN messages for Core node
+    * /anchor/from_vic/arm
+        - VicCAN messages for Arm node
+    * /anchor/from_vic/bio
+        - VicCAN messages for Bio node
+
+    Subscribers:
+    * /anchor/from_vic/mock_mcu
+        - For testing without an actual MCU, publish strings here as if they came from an MCU
+    * /anchor/to_vic/relay
+        - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+    * /anchor/to_vic/relay_string
+        - Publish raw strings to this topic to send directly to the MCU for debugging
+    """
 
-Subscribers:
-  * /anchor/from_vic/mock_mcu
-    - For testing without an actual MCU, publish strings here as if they came from an MCU
-  * /anchor/to_vic/relay
-    - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
-  * /anchor/to_vic/relay_string
-    - 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'
+        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.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
 
-        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)
+        # Serial port override
+        if port_override := os.getenv("PORT_OVERRIDE"):
+            self.serial_port = port_override
 
+        ##################################################
+        # Serial MCU Discovery
 
-        # 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)
+        # If there was not a port override, look for a MCU over USB for Serial.
+        if self.serial_port is None:
+            comports = serial.tools.list_ports.comports()
+            real_ports = list(
+                filter(
+                    lambda p: p.vid is not None
+                    and p.pid is not None
+                    and p.device is not None,
+                    comports,
+                )
+            )
+            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
 
-        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)
+            if len(recog_ports) == 1:  # Found singular recognized MCU
+                found_port = recog_ports[0]
+                self.get_logger().info(
+                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
+                )
+                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
+            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
+                # Kinda jank log message
+                self.get_logger().error(
+                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            elif (
+                len(recog_ports) == 0 and len(real_ports) > 0
+            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
+                self.get_logger().error(
+                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
+                )
+                # Don't set self.serial_port; later if-statement will exit()
+            else:  # Found jack shit
+                self.get_logger().error("No valid Serial ports specified or found.")
+                # Don't set self.serial_port; later if-statement will exit()
 
+        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
         # Loop through all serial devices on the computer to check for the MCU
-        self.port = None
-        # self.port = "/tmp/ttyACM9"  # Fake port, for debugging
-        ports = SerialRelay.list_serial_ports()
-        for i in range(4):
-            if self.port is not None:
-                break
-            for port in ports:
-                try:
-                    # connect and send a ping command
-                    ser = serial.Serial(port, 115200, timeout=1)
-                    #(f"Checking port {port}...")
-                    ser.write(b"ping\n")
-                    response = ser.read_until(bytes("\n", "utf8"))
+        if self.serial_port is None:
+            self.get_logger().warning("Falling back to legacy MCU discovery...")
+            ports = Anchor.list_serial_ports()
+            for _ in range(4):
+                if self.serial_port is not None:
+                    break
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        # (f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until(bytes("\n", "utf8"))
 
-                    # if pong is in response, then we are talking with the MCU
-                    if b"pong" in response:
-                        self.port = port
-                        self.get_logger().info(f"Found MCU at {self.port}!")
-                        break
-                except:
-                    pass
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.serial_port = port
+                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                            break
+                    except:
+                        pass
 
-        if self.port is None:
-            self.get_logger().info("Unable to find MCU...")
+        # If port is still None then we ain't finding no mcu
+        if self.serial_port is None:
+            self.get_logger().error("Unable to find MCU. Exiting...")
             time.sleep(1)
             sys.exit(1)
+        # Found a Serial port, try to open it; above code has not officially opened a Serial port
+        else:
+            self.get_logger().debug(
+                f"Attempting to open Serial port '{self.serial_port}'..."
+            )
+            try:
+                self.serial_interface = serial.Serial(
+                    self.serial_port, 115200, timeout=1
+                )
 
-        self.ser = serial.Serial(self.port, 115200)
-        self.get_logger().info(f"Enabling Relay Mode")
-        self.ser.write(b"can_relay_mode,on\n")
+                # Attempt to get name of connected MCU
+                self.serial_interface.write(
+                    b"can_relay_mode,on\n"
+                )  # can_relay_ready,[mcu]
+                mcu_name: str = ""
+                for _ in range(4):
+                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
+                    try:
+                        if b"can_relay_ready" in response:
+                            args: list[str] = response.decode("utf8").strip().split(",")
+                            if len(args) == 2:
+                                mcu_name = args[1]
+                            break
+                    except UnicodeDecodeError:
+                        pass  # ignore malformed responses
+                self.get_logger().info(
+                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
+                )
+
+            except serial.SerialException as e:
+                self.get_logger().error(
+                    f"Could not open Serial port '{self.serial_port}' for reason:"
+                )
+                self.get_logger().error(e.strerror)
+                time.sleep(1)
+                sys.exit(1)
+
+        # Close serial port on exit
         atexit.register(self.cleanup)
 
+        ##################################################
+        # ROS2 Topic Setup
 
-    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
-        except KeyboardInterrupt:
-            sys.exit(0)
+        # 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 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")
-            
+            output = str(self.serial_interface.readline(), "utf8")
+
             if output:
                 self.relay_fromvic(output)
                 # All output over debug temporarily
-                #self.get_logger().info(f"[MCU] {output}")
+                # 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
@@ -135,14 +232,20 @@ class SerialRelay(Node):
         except serial.SerialException as e:
             print(f"SerialException: {e}")
             print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except TypeError as e:
             print(f"TypeError: {e}")
             print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
+            try:
+                if self.serial_interface.is_open:
+                    self.serial_interface.close()
+            except:
+                pass
             exit(1)
         except Exception as e:
             print(f"Exception: {e}")
@@ -151,35 +254,67 @@ 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
         output += "\n"
         # self.get_logger().info(f"VicCAN relay to MCU: {output}")
-        self.ser.write(bytes(output, "utf8"))
+        self.serial_interface.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,12 +324,11 @@ 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.ser.write(bytes(message, "utf8"))
+        # self.get_logger().info(f"Sending command to MCU: {msg}")
+        self.serial_interface.write(bytes(message, "utf8"))
 
     @staticmethod
     def list_serial_ports():
@@ -202,25 +336,30 @@ class SerialRelay(Node):
 
     def cleanup(self):
         print("Cleaning up before terminating...")
-        if self.ser.is_open:
-            self.ser.close()
-
-def myexcepthook(type, value, tb):
-    print("Uncaught exception:", type, value)
-    if serial_pub:
-        serial_pub.cleanup()
+        if self.serial_interface.is_open:
+            self.serial_interface.close()
 
 
 def main(args=None):
-    rclpy.init(args=args)
-    sys.excepthook = myexcepthook
+    try:
+        rclpy.init(args=args)
+        anchor_node = Anchor()
 
-    global serial_pub
+        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
+        thread.start()
 
-    serial_pub = SerialRelay()
-    serial_pub.run()
+        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
+        while rclpy.ok():
+            anchor_node.read_MCU()  # Check the MCU for updates
+            rate.sleep()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal, shutting down...")
+    finally:
+        rclpy.try_shutdown()
 
-if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+
+if __name__ == "__main__":
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

src/anchor_pkg/launch/rover.launch.py

@@ -0,0 +1,189 @@
+#!/usr/bin/env python3
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    Shutdown,
+    IncludeLaunchDescription,
+)
+from launch.substitutions import (
+    LaunchConfiguration,
+    ThisLaunchFileDir,
+    PathJoinSubstitution,
+)
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+# Prevent making __pycache__ directories
+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},
+                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
+                ],
+                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",
+    )
+
+    ros2_control_arg = DeclareLaunchArgument(
+        "use_ros2_control",
+        default_value="false",
+        description="Whether to use DiffDriveController for driving instead of direct Twist",
+    )
+
+    rsp = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("core_description"),
+                    "launch",
+                    "robot_state_publisher.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
+    )
+
+    controllers = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("core_description"),
+                    "launch",
+                    "spawn_controllers.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
+    )
+
+    return LaunchDescription(
+        [
+            declare_arg,
+            ros2_control_arg,
+            rsp,
+            controllers,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

src/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>

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"],
     },
 )

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()

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, 2
+        )
 
-        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()

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

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>

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

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",
         ],
     },
 )

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>

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>

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()

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>

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"],
     },
 )

src/core_gazebo/CMakeLists.txt

@@ -1,36 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(core_gazebo)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-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}
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-ament_package()

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
-  
-

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

src/core_gazebo/package.xml

@@ -1,18 +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_gazebo</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="ds0196@uah.edu">David</maintainer>
-  <license>AGPL-3.0-only</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

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>

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>

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()

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,19 +28,33 @@ 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,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # deadline=Duration(seconds=1),
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # 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,119 @@ 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}")
 
+        self.declare_parameter("use_ros2_control", False)
+        self.use_ros2_control = self.get_parameter("use_ros2_control").value
+        self.get_logger().info(f"Use ros2_control: {self.use_ros2_control}")
 
         ##################################################
         # Topics
 
         # 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)
-        # 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)
-        # 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
+        if self.use_ros2_control:
+            # Joint state control for topic-based controller
+            self.joint_command_sub_ = self.create_subscription(
+                JointState, "/core/joint_commands", self.joint_command_callback, 2
+            )
+        else:
+            # 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, 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
+            )
+            # 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
 
         # 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, "/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
+
+        if not self.use_ros2_control:
+            # /core/control
+            self.control_sub = self.create_subscription(
+                CoreControl, "/core/control", self.send_controls, 10
+            )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
         # /core/feedback
-        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
+        self.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 +183,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 +198,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 +216,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 +254,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 +268,63 @@ 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 joint_command_callback(self, msg: JointState):
+        # So... topic based control node publishes JointState messages over /joint_commands
+        #  with len(msg.name) == 5 and len(msg.velocity) == 4... all 5 non-fixed joints
+        #  are included in msg.name, but ig it is implied that msg.velocity only
+        #  includes velocities for the commanded joints (ros__parameters.joints).
+        # So, this will be much more hacky and less adaptable than I would like it to be.
+        if len(msg.name) != 5 or len(msg.velocity) != 4 or len(msg.position) != 0:
+            self.get_logger().warning(
+                f"Received joint control message with unexpected number of joints. Ignoring."
+            )
+            return
+        if msg.name != [
+            "left_suspension_joint",
+            "bl_wheel_joint",
+            "br_wheel_joint",
+            "fl_wheel_joint",
+            "fr_wheel_joint",
+        ]:
+            self.get_logger().warning(
+                f"Received joint control message with unexpected name[]. Ignoring."
+            )
+            return
+
+        (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
+
+        bl_rpm = radps_to_rpm(bl_vel) * CORE_GEAR_RATIO
+        br_rpm = radps_to_rpm(br_vel) * CORE_GEAR_RATIO
+        fl_rpm = radps_to_rpm(fl_vel) * CORE_GEAR_RATIO
+        fr_rpm = radps_to_rpm(fr_vel) * CORE_GEAR_RATIO
+
+        self.send_viccan(
+            20, [fl_rpm, bl_rpm, fr_rpm, br_rpm]
+        )  # order expected by embedded
 
     def cmd_vel_callback(self, msg: TwistStamped):
         linear = msg.twist.linear.x
@@ -232,20 +335,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 +361,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 +378,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 +459,155 @@ 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_wheel_joint"]
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                        joint_state_msg.name = ["bl_wheel_joint"]
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                        joint_state_msg.name = ["fr_wheel_joint"]
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                        joint_state_msg.name = ["br_wheel_joint"]
+                    case _:
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
+                        )
+                        return
+
+                # make the fucking shit work
+                joint_state_msg.name.append("left_suspension_joint")
+                joint_state_msg.position.append(0.0)
+                joint_state_msg.velocity.append(0.0)
+
+                joint_state_msg.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(joint_state_msg)
+            case _:
+                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 +622,30 @@ 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 radps_to_rpm(radps: float):
+    return radps * 60 / (2 * pi)
+
+
 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()

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()

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?"

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)
 

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>

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",
         ],
     },
 )

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', ]

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

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

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

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>

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

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'
-)

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,,,,,,,,,,,,

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>

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>

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",
         ],
     },

src/headless_pkg/src/headless_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
@@ -7,37 +8,41 @@ import signal
 import time
 import atexit
 
-import serial
 import os
 import sys
 import threading
 import glob
+import pwd
+import grp
+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,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # deadline=Duration(seconds=1),
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"
@@ -48,6 +53,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...")
@@ -61,21 +74,49 @@ class Headless(Node):
             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()}')
+        id = 0
+        while True:
+            self.num_gamepads = pygame.joystick.get_count()
+            if id >= self.num_gamepads:
+                self.get_logger().fatal("Ran out of controllers to try")
+                sys.exit(1)
+
+            try:
+                self.gamepad = pygame.joystick.Joystick(id)
+                self.gamepad.init()
+            except Exception as e:
+                self.get_logger().error("Error when initializing gamepad")
+                self.get_logger().error(e)
+                id += 1
+                continue
+            print(f"Gamepad Found: {self.gamepad.get_name()}")
+
+            if self.gamepad.get_numhats() == 0 or self.gamepad.get_numaxes() < 5:
+                self.get_logger().error("Controller not correctly initialized.")
+                if not is_user_in_group("input"):
+                    self.get_logger().warning(
+                        "If using NixOS, you may need to add yourself to the 'input' group."
+                    )
+                if is_user_in_group("plugdev"):
+                    self.get_logger().warning(
+                        "If using NixOS, you may need to remove yourself from the 'plugdev' group."
+                    )
+            else:
+                break
+            id += 1
 
-        # 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 +125,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,14 +138,14 @@ 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:
+        if pygame.joystick.get_count() != self.num_gamepads:
             print("Gamepad disconnected. Exiting...")
             # Send one last zero control message
             self.core_publisher.publish(CORE_STOP_MSG)
@@ -116,7 +156,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 +170,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 +180,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 +189,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 +209,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 +232,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 +260,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 +274,6 @@ class Headless(Node):
             elif dpad_input[0] == -1:
                 arm_input.axis0 = -1
 
-
             if right_bumper:  # Control end effector
 
                 # Effector yaw
@@ -246,23 +288,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,18 +320,44 @@ 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
 
 
-def main(args=None):
-    rclpy.init(args=args)
-    node = Headless()
-    rclpy.spin(node)
-    rclpy.shutdown()
+def is_user_in_group(group_name: str) -> bool:
+    # Copied from https://zetcode.com/python/os-getgrouplist/
+    try:
+        username = os.getlogin()
 
-if __name__ == '__main__':
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+        # Get group ID from name
+        group_info = grp.getgrnam(group_name)
+        target_gid = group_info.gr_gid
+
+        # Get user's groups
+        user_info = pwd.getpwnam(username)
+        user_groups = os.getgrouplist(username, user_info.pw_gid)
+
+        return target_gid in user_groups
+    except KeyError:
+        return False
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+
+        node = Headless()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal. Exiting...")
+    finally:
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

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

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>

src/servo_arm_twist_pkg/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.22)
+project(servo_arm_twist_pkg)
+
+# C++ Libraries #################################################
+
+# Core C++ library for calculations and collision checking.
+# Provides interface used by the component node.
+set(SERVO_LIB_NAME servo_arm_twist_lib)
+
+# Pose Tracking
+set(POSE_TRACKING pose_tracking)
+
+# Component Nodes (Shared libraries) ############################
+set(SERVO_COMPONENT_NODE servo_node)
+set(SERVO_CONTROLLER_INPUT servo_controller_input)
+
+# Executable Nodes ##############################################
+set(SERVO_NODE_MAIN_NAME servo_node_main)
+set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
+set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
+
+#################################################################
+
+# Common cmake code applied to all moveit packages
+find_package(moveit_common REQUIRED)
+moveit_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
+  control_toolbox
+  geometry_msgs
+  moveit_core
+  moveit_msgs
+  moveit_ros_planning
+  pluginlib
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  std_msgs
+  std_srvs
+  tf2_eigen
+  trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+#####################
+## Component Nodes ##
+#####################
+
+# Add executable for using a controller
+add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
+ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
+
+#############
+## Install ##
+#############
+
+# Install Libraries
+install(
+  TARGETS
+    ${SERVO_CONTROLLER_INPUT}
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+# Install Binaries
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()

src/servo_arm_twist_pkg/README.md

@@ -0,0 +1,3 @@
+# Moveit Servo
+
+See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.

src/servo_arm_twist_pkg/package.xml

@@ -0,0 +1,58 @@
+<?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>servo_arm_twist_pkg</name>
+  <version>2.5.9</version>
+  <description>Provides real-time manipulator Cartesian and joint servoing.</description>
+  <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
+  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
+  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+
+  <license>BSD 3-Clause</license>
+
+  <url type="website">https://ros-planning.github.io/moveit_tutorials</url>
+
+  <author>Brian O'Neil</author>
+  <author email="andyz@utexas.edu">Andy Zelenak</author>
+  <author>Blake Anderson</author>
+  <author email="alex@machinekoder.com">Alexander Rössler</author>
+  <author email="tyler@picknik.ai">Tyler Weaver</author>
+  <author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_common</depend>
+
+  <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>trajectory_msgs</depend>
+
+  <exec_depend>gripper_controllers</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>launch_param_builder</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>

src/servo_arm_twist_pkg/src/joystick_twist.cpp

@@ -0,0 +1,271 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*      Title     : joystick_servo_example.cpp
+ *      Project   : servo_arm_twist_pkg
+ *      Created   : 08/07/2020
+ *      Author    : Adam Pettinger
+ */
+
+#include <sensor_msgs/msg/joy.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <control_msgs/msg/joint_jog.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <rclcpp/client.hpp>
+#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/qos_event.hpp>
+#include <rclcpp/subscription.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/utilities.hpp>
+#include <thread>
+
+// We'll just set up parameters here
+const std::string JOY_TOPIC = "/joy";
+const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
+const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
+const std::string EEF_FRAME_ID = "End_Effector";
+const std::string BASE_FRAME_ID = "base_link";
+
+// Enums for button names -> axis/button array index
+// For XBOX 1 controller
+enum Axis
+{
+    LEFT_STICK_X = 0,
+    LEFT_STICK_Y = 1,
+    LEFT_TRIGGER = 2,
+    RIGHT_STICK_X = 3,
+    RIGHT_STICK_Y = 4,
+    RIGHT_TRIGGER = 5,
+    D_PAD_X = 6,
+    D_PAD_Y = 7
+};
+enum Button
+{
+    A = 0,
+    B = 1,
+    X = 2,
+    Y = 3,
+    LEFT_BUMPER = 4,
+    RIGHT_BUMPER = 5,
+    CHANGE_VIEW = 6,
+    MENU = 7,
+    HOME = 8,
+    LEFT_STICK_CLICK = 9,
+    RIGHT_STICK_CLICK = 10
+};
+
+// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
+// This will map the default values for the axes
+std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
+std::map<Button, double> BUTTON_DEFAULTS;
+
+// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
+// functions
+/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
+ * @param axes The vector of continuous controller joystick axes
+ * @param buttons The vector of discrete controller button values
+ * @param twist A TwistStamped message to update in prep for publishing
+ * @param joint A JointJog message to update in prep for publishing
+ * @return return true if you want to publish a Twist, false if you want to publish a JointJog
+ */
+bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
+                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
+                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
+{
+    // // Give joint jogging priority because it is only buttons
+    // // If any joint jog command is requested, we are only publishing joint commands
+    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
+    // {
+    //   // Map the D_PAD to the proximal joints
+    //   joint->joint_names.push_back("panda_joint1");
+    //   joint->velocities.push_back(axes[D_PAD_X]);
+    //   joint->joint_names.push_back("panda_joint2");
+    //   joint->velocities.push_back(axes[D_PAD_Y]);
+
+    //   // Map the diamond to the distal joints
+    //   joint->joint_names.push_back("panda_joint7");
+    //   joint->velocities.push_back(buttons[B] - buttons[X]);
+    //   joint->joint_names.push_back("panda_joint6");
+    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
+    //   return false;
+    // }
+
+    // The bread and butter: map buttons to twist commands
+    twist->twist.linear.z = axes[RIGHT_STICK_Y];
+    twist->twist.linear.y = axes[RIGHT_STICK_X];
+
+    double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
+    double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
+    twist->twist.linear.x = lin_x_right + lin_x_left;
+
+    twist->twist.angular.y = axes[LEFT_STICK_Y];
+    twist->twist.angular.x = axes[LEFT_STICK_X];
+
+    double roll_positive = buttons[RIGHT_BUMPER];
+    double roll_negative = -1 * (buttons[LEFT_BUMPER]);
+    twist->twist.angular.z = roll_positive + roll_negative;
+
+    return true;
+}
+
+/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
+ * @param frame_name Set the command frame to this
+ * @param buttons The vector of discrete controller button values
+ */
+void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
+{
+    if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
+        frame_name = BASE_FRAME_ID;
+    else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
+        frame_name = EEF_FRAME_ID;
+}
+
+namespace servo_arm_twist_pkg
+{
+class JoyToServoPub : public rclcpp::Node
+{
+public:
+    JoyToServoPub(const rclcpp::NodeOptions& options)
+        : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
+    {
+        // Setup pub/sub
+        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
+            JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
+            [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
+
+        twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
+        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
+        // collision_pub_ =
+        //     this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
+
+        // Create a service client to start the ServoNode
+        servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
+        servo_start_client_->wait_for_service(std::chrono::seconds(1));
+        servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
+
+        // // Load the collision scene asynchronously
+        // collision_pub_thread_ = std::thread([this]() {
+        //   rclcpp::sleep_for(std::chrono::seconds(3));
+        //   // Create collision object, in the way of servoing
+        //   moveit_msgs::msg::CollisionObject collision_object;
+        //   collision_object.header.frame_id = "panda_link0";
+        //   collision_object.id = "box";
+
+        //   shape_msgs::msg::SolidPrimitive table_1;
+        //   table_1.type = table_1.BOX;
+        //   table_1.dimensions = { 0.4, 0.6, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_1_pose;
+        //   table_1_pose.position.x = 0.6;
+        //   table_1_pose.position.y = 0.0;
+        //   table_1_pose.position.z = 0.4;
+
+        //   shape_msgs::msg::SolidPrimitive table_2;
+        //   table_2.type = table_2.BOX;
+        //   table_2.dimensions = { 0.6, 0.4, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_2_pose;
+        //   table_2_pose.position.x = 0.0;
+        //   table_2_pose.position.y = 0.5;
+        //   table_2_pose.position.z = 0.25;
+
+        //   collision_object.primitives.push_back(table_1);
+        //   collision_object.primitive_poses.push_back(table_1_pose);
+        //   collision_object.primitives.push_back(table_2);
+        //   collision_object.primitive_poses.push_back(table_2_pose);
+        //   collision_object.operation = collision_object.ADD;
+
+        //   moveit_msgs::msg::PlanningSceneWorld psw;
+        //   psw.collision_objects.push_back(collision_object);
+
+        //   auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
+        //   ps->world = psw;
+        //   ps->is_diff = true;
+        //   collision_pub_->publish(std::move(ps));
+        // });
+    }
+
+    // ~JoyToServoPub() override
+    // {
+    //     if (collision_pub_thread_.joinable())
+    //         collision_pub_thread_.join();
+    // }
+
+    void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
+    {
+        // Create the messages we might publish
+        auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+        auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
+
+        // This call updates the frame for twist commands
+        updateCmdFrame(frame_to_publish_, msg->buttons);
+
+        // Convert the joystick message to Twist or JointJog and publish
+        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
+        {
+            // publish the TwistStamped
+            twist_msg->header.frame_id = frame_to_publish_;
+            twist_msg->header.stamp = this->now();
+            twist_pub_->publish(std::move(twist_msg));
+        }
+        // else
+        // {
+        //     // publish the JointJog
+        //     joint_msg->header.stamp = this->now();
+        //     joint_msg->header.frame_id = "panda_link3";
+        //     joint_pub_->publish(std::move(joint_msg));
+        // }
+    }
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
+    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
+    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
+
+    std::string frame_to_publish_;
+
+    // std::thread collision_pub_thread_;
+};  // class JoyToServoPub
+
+}  // namespace servo_arm_twist_pkg
+
+// Register the component with class_loader
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)