finish adding parameters

Headless too

.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,220 @@
-# 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)
+- [Graphs](#graphs)
+  - [Full System](#full-system)
+  - [Individual Nodes](#individual-nodes)
+- [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
+### Using the Mock Connector
+
+Anchor provides a mock connector meant for testing and scripting purposes. You can select the mock connector by running anchor with this command:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:="mock"
+```
+
+You can see all data sent to it in a string format with this command:
+
+```bash
+$ ros2 topic echo /anchor/to_vic/debug
+```
+
+### 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 `serial_override` ROS2 parameter to point it to the fake serial port, like so:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:=serial serial_override:=/tmp/ttyACM9
+```
+
+### Testing CAN
+
+You can create a virtual CAN network by using the following commands to create and then enable it:
+
+```bash
+sudo ip link add dev vcan0 type vcan
+sudo ip link set vcan0 up
+```
+
+When you go to run anchor, use the `can_override` ROS2 parameter to point it to the virtual CAN network, like so:
+
+```bash
+$ ros2 launch anchor_pkg rover.launch.py connector:=can can_override:=vcan0
+```
+
+Once you're done, you should delete the virtual network so that anchor doesn't get confused if you plug in a real CAN adapter:
+
+```bash
+$ sudo ip link delete vcan0
+```
+
+### Connecting the GuliKit Controller
+
+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.
+
+## Graphs
+
+### Full System
+
+> **Anchor stand-alone** (`ros2 launch anchor_pkg rover.launch.py`)
+>
+> ![rqt_graph of Anchor by itself, ran with command: ros2 launch anchor_pkg rover.launch.py](./docs-resources/graph-anchor-standalone.png)
+
+> **Anchor with [basestation-classic](https://github.com/SHC-ASTRA/basestation-classic)**
+>
+> ![rqt_graph of Anchor ran with the same command as above, talking to basestation-classic](./docs-resources/graph-anchor-w-basestation-classic.png)
+
+> **Anchor with Headless** (`ros2 run headless_pkg headless_full`)
+>
+> ![rqt_graph of Anchor ran with Headless](./docs-resources/graph-anchor-w-headless.png)
+
+### Individual Nodes
+
+> **Anchor** (`ros2 run anchor_pkg anchor`)
+>
+> ![rqt_graph of Anchor node running by itself](./docs-resources/graph-anchor-anchor-standalone.png)
+
+> **Core** (`ros2 run core_pkg core --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Core node running by itself](./docs-resources/graph-anchor-core-standalone.png)
+
+> **Arm** (`ros2 run arm_pkg arm --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Arm node running by itself](./docs-resources/graph-anchor-arm-standalone.png)
+
+> **Bio** (`ros2 run bio_pkg bio --ros-args -p launch_mode:=anchor`)
+>
+> ![rqt_graph of Bio node running by itself](./docs-resources/graph-anchor-bio-standalone.png)
+
+## 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": {
@@ -60,7 +60,8 @@
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
-        ]
+        ],
+        "treefmt-nix": "treefmt-nix"
       }
     },
     "systems": {
@@ -77,6 +78,26 @@
         "repo": "default",
         "type": "github"
       }
+    },
+    "treefmt-nix": {
+      "inputs": {
+        "nixpkgs": [
+          "nixpkgs"
+        ]
+      },
+      "locked": {
+        "lastModified": 1773297127,
+        "narHash": "sha256-6E/yhXP7Oy/NbXtf1ktzmU8SdVqJQ09HC/48ebEGBpk=",
+        "owner": "numtide",
+        "repo": "treefmt-nix",
+        "rev": "71b125cd05fbfd78cab3e070b73544abe24c5016",
+        "type": "github"
+      },
+      "original": {
+        "owner": "numtide",
+        "repo": "treefmt-nix",
+        "type": "github"
+      }
     }
   },
   "root": "root",

flake.nix

@@ -1,30 +1,100 @@
 {
+  description = "Development environment for ASTRA Anchor";
+
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
-    nixpkgs.follows = "nix-ros-overlay/nixpkgs";  # IMPORTANT!!!
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
+
+    treefmt-nix = {
+      url = "github:numtide/treefmt-nix";
+      inputs.nixpkgs.follows = "nixpkgs";
+    };
   };
-  outputs = { self, nix-ros-overlay, nixpkgs }:
-    nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (system:
+
+  outputs =
+    {
+      self,
+      nix-ros-overlay,
+      nixpkgs,
+      ...
+    }@inputs:
+    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
+            (python313.withPackages (
+              p: with p; [
+                pyserial
+                python-can
+                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
+          '';
         };
-      });
+
+        formatter = (inputs.treefmt-nix.lib.evalModule pkgs ./treefmt.nix).config.build.wrapper;
+      }
+    );
+
   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,226 +1,236 @@
 import rclpy
 from rclpy.node import Node
-from std_srvs.srv import Empty
+from rclpy.executors import ExternalShutdownException
+from rcl_interfaces.msg import ParameterDescriptor, ParameterType
 
 import signal
-import time
 import atexit
 
-import serial
-import os
-import sys
+from .connector import (
+    Connector,
+    MockConnector,
+    SerialConnector,
+    CANConnector,
+    NoValidDeviceException,
+    NoWorkingDeviceException,
+)
+from .convert import string_to_viccan, viccan_to_string
 import threading
-import glob
 
-from std_msgs.msg import String, Header
-from ros2_interfaces_pkg.msg import VicCAN
-
-serial_pub = None
-thread = None
+from astra_msgs.msg import VicCAN
+from std_msgs.msg import String
 
 
-"""
-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
+    * /anchor/to_vic/debug
+        - A string copy of the messages published to ./relay are published here
+
+    Subscribers:
+    * /anchor/from_vic/mock_mcu
+        - For testing without an actual MCU, publish strings here as if they came from an MCU
+    * /anchor/to_vic/relay
+        - Core, Arm, and Bio publish VicCAN messages to this topic to send to the MCU
+    * /anchor/to_vic/relay_string
+        - Send raw strings to connectors. Does not work for connectors that require conversion (like CANConnector)
+    """
+
+    connector: Connector
 
-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")
 
-        # 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)
+        logger = self.get_logger()
 
-        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)
+        # ROS2 Parameter Setup
 
+        self.declare_parameter(
+            "connector",
+            "auto",
+            ParameterDescriptor(
+                name="connector",
+                description="Declares which MCU connector should be used. Defaults to 'auto'.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be 'serial', 'can', 'mock', or 'auto'.",
+            ),
+        )
 
-        # 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.declare_parameter(
+            "can_override",
+            "",
+            ParameterDescriptor(
+                name="can_override",
+                description="Overrides which CAN channel will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid CAN network that shows up in `ip link show`.",
+            ),
+        )
 
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
+        self.declare_parameter(
+            "serial_override",
+            "",
+            ParameterDescriptor(
+                name="serial_override",
+                description="Overrides which serial port will be used. Defaults to ''.",
+                type=ParameterType.PARAMETER_STRING,
+                additional_constraints="Must be a valid path to a serial device file that shows up in `ls /dev/tty*`.",
+            ),
+        )
 
-        # Create a subscriber 
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
-
-        # 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:
+        # Determine which connector to use. Options are Mock, Serial, and CAN
+        connector_select = (
+            self.get_parameter("connector").get_parameter_value().string_value
+        )
+        can_override = (
+            self.get_parameter("can_override").get_parameter_value().string_value
+        )
+        serial_override = (
+            self.get_parameter("serial_override").get_parameter_value().string_value
+        )
+        match connector_select:
+            case "serial":
+                logger.info("using serial connector")
+                self.connector = SerialConnector(
+                    logger, self.get_clock(), serial_override
+                )
+            case "can":
+                logger.info("using CAN connector")
+                self.connector = CANConnector(logger, self.get_clock(), can_override)
+            case "mock":
+                logger.info("using mock connector")
+                self.connector = MockConnector(logger, self.get_clock())
+            case "auto":
+                logger.info("automatically determining connector")
                 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"))
+                    logger.info("trying CAN connector")
+                    self.connector = CANConnector(
+                        logger, self.get_clock(), can_override
+                    )
+                except (NoValidDeviceException, NoWorkingDeviceException, TypeError):
+                    logger.info("CAN connector failed, trying serial connector")
+                    self.connector = SerialConnector(
+                        logger, self.get_clock(), serial_override
+                    )
+            case _:
+                logger.fatal(
+                    f"invalid value for connector parameter: {connector_select}"
+                )
+                exit(1)
 
-                    # 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
+        # ROS2 Topic Setup
 
-        if self.port is None:
-            self.get_logger().info("Unable to find MCU...")
-            time.sleep(1)
-            sys.exit(1)
+        # Publishers
+        self.fromvic_debug_pub_ = self.create_publisher(  # only used by serial
+            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,
+        )
+        # Debug publisher
+        self.tovic_debug_pub_ = self.create_publisher(
+            VicCAN,
+            "/anchor/to_vic/debug",
+            20,
+        )
 
-        self.ser = serial.Serial(self.port, 115200)
-        self.get_logger().info(f"Enabling Relay Mode")
-        self.ser.write(b"can_relay_mode,on\n")
+        # Subscribers
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN,
+            "/anchor/to_vic/relay",
+            self.write_connector,
+            20,
+        )
+        self.mock_mcu_sub_ = self.create_subscription(
+            String,
+            "/anchor/from_vic/mock_mcu",
+            self.on_mock_fromvic,
+            20,
+        )
+        self.tovic_string_sub_ = self.create_subscription(
+            String,
+            "/anchor/to_vic/relay_string",
+            self.connector.write_raw,
+            20,
+        )
+
+        # Close devices on exit
         atexit.register(self.cleanup)
 
+    def cleanup(self):
+        self.connector.cleanup()
 
-    def 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)
+    def read_connector(self):
+        """Check the connector for new data from the MCU, and publish string to appropriate topics"""
+        viccan, raw = self.connector.read()
 
-    def read_MCU(self):
-        """ Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
-        try:
-            output = str(self.ser.readline(), "utf8")
-            
-            if output:
-                self.relay_fromvic(output)
-                # All output over debug temporarily
-                #self.get_logger().info(f"[MCU] {output}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,core"):
-                    self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
-                    self.arm_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
-                    self.bio_pub.publish(msg)
-                # msg = String()
-                # msg.data = output
-                # self.debug_pub.publish(msg)
-                return
-        except serial.SerialException as e:
-            print(f"SerialException: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            exit(1)
-        except TypeError as e:
-            print(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            exit(1)
-        except Exception as e:
-            print(f"Exception: {e}")
-            # print("Closing serial port.")
-            # if self.ser.is_open:
-            #     self.ser.close()
-            # exit(1)
+        if raw:
+            self.fromvic_debug_pub_.publish(String(data=raw))
 
+        if viccan:
+            self.relay_fromvic(viccan)
+
+    def write_connector(self, msg: VicCAN):
+        """Write to the connector and send a copy to /anchor/to_vic/debug"""
+        self.connector.write(msg)
+        self.tovic_debug_pub_.publish(viccan_to_string(msg))
+
+    def relay_fromvic(self, msg: VicCAN):
+        """Relay a message from the MCU to the appropriate VicCAN topic"""
+        if msg.mcu_name == "core":
+            self.fromvic_core_pub_.publish(msg)
+        elif msg.mcu_name == "arm" or msg.mcu_name == "digit":
+            self.fromvic_arm_pub_.publish(msg)
+        elif msg.mcu_name == "citadel" or msg.mcu_name == "digit":
+            self.fromvic_bio_pub_.publish(msg)
 
     def on_mock_fromvic(self, msg: String):
-        """ 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 """
-        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"))
-
-    def relay_fromvic(self, msg: str):
-        """ 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":
-            return
-
-        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")
-
-        # self.get_logger().info(f"Relaying from MCU: {output}")
-        if output.mcu_name == "core":
-            self.fromvic_core_pub_.publish(output)
-        elif output.mcu_name == "arm" or output.mcu_name == "digit":
-            self.fromvic_arm_pub_.publish(output)
-        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 """
-        message = msg.data
-        #self.get_logger().info(f"Sending command to MCU: {msg}")
-        self.ser.write(bytes(message, "utf8"))
-
-    @staticmethod
-    def list_serial_ports():
-        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-
-    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()
+        """Relay a message as if it came from the MCU"""
+        viccan = string_to_viccan(
+            msg.data,
+            "mock",
+            self.get_logger(),
+            self.get_clock().now().to_msg(),
+        )
+        if viccan:
+            self.relay_fromvic(viccan)
 
 
 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()
-
-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()
+        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
+        while rclpy.ok():
+            anchor_node.read_connector()  # Check the connector for updates
+            rate.sleep()
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal, shutting down...")
+    finally:
+        rclpy.try_shutdown()

src/anchor_pkg/anchor_pkg/connector.py

@@ -0,0 +1,472 @@
+from abc import ABC, abstractmethod
+from astra_msgs.msg import VicCAN
+from rclpy.clock import Clock
+from rclpy.impl.rcutils_logger import RcutilsLogger
+from .convert import string_to_viccan as _string_to_viccan, viccan_to_string
+
+# CAN
+import can
+import can.interfaces.socketcan
+import struct
+
+# Serial
+import serial
+import serial.tools.list_ports
+
+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
+]
+BAUD_RATE = 115200
+
+
+class NoValidDeviceException(Exception):
+    pass
+
+
+class NoWorkingDeviceException(Exception):
+    pass
+
+
+class MultipleValidDevicesException(Exception):
+    pass
+
+
+class DeviceClosedException(Exception):
+    pass
+
+
+class Connector(ABC):
+    logger: RcutilsLogger
+    clock: Clock
+
+    def string_to_viccan(self, msg: str, mcu_name: str):
+        """function currying so that we do not need to pass logger and clock every time"""
+        return _string_to_viccan(
+            msg,
+            mcu_name,
+            self.logger,
+            self.clock.now().to_msg(),
+        )
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
+        self.logger = logger
+        self.clock = clock
+
+    @abstractmethod
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        """
+        Must return a tuple of (VicCAN, debug message or string repr of VicCAN)
+        """
+        pass
+
+    @abstractmethod
+    def write(self, msg: VicCAN):
+        pass
+
+    @abstractmethod
+    def write_raw(self, msg: str):
+        pass
+
+    def cleanup(self):
+        pass
+
+
+class SerialConnector(Connector):
+    port: str
+    mcu_name: str
+    serial_interface: serial.Serial
+
+    def __init__(self, logger: RcutilsLogger, clock: Clock, serial_override: str = ""):
+        super().__init__(logger, clock)
+
+        ports = self._find_ports()
+        mcu_name: str | None = None
+
+        if serial_override:
+            logger.warn(
+                f"using serial_override: `{serial_override}`! this will bypass several checks."
+            )
+            ports = [serial_override]
+            mcu_name = "override"
+
+        if len(ports) <= 0:
+            raise NoValidDeviceException("no valid serial device found")
+        if (l := len(ports)) > 1:
+            raise MultipleValidDevicesException(
+                f"too many ({l}) valid serial devices found"
+            )
+
+        # check each of our ports to make sure one of them is responding
+        port = ports[0]
+        # we might already have a name by now if we overrode earlier
+        mcu_name = mcu_name or self._get_name(port)
+        if not mcu_name:
+            raise NoWorkingDeviceException(
+                f"found {port}, but it did not respond with its name"
+            )
+
+        self.port = port
+        self.mcu_name = mcu_name
+
+        # if we fail at this point, it should crash because we've already tested the port
+        self.serial_interface = serial.Serial(self.port, BAUD_RATE, timeout=1)
+
+    def _find_ports(self) -> list[str]:
+        """
+        Finds all valid ports but does not test them
+
+        returns: all valid ports
+        """
+        comports = serial.tools.list_ports.comports()
+        valid_ports = list(
+            map(  # get just device strings
+                lambda p: p.device,
+                filter(  # make sure we have a known device
+                    lambda p: (p.vid, p.pid) in KNOWN_USBS and p.device is not None,
+                    comports,
+                ),
+            )
+        )
+        self.logger.info(f"found valid MCU ports: [ {', '.join(valid_ports)} ]")
+        return valid_ports
+
+    def _get_name(self, port: str) -> str | None:
+        """
+        Get the name of the MCU (if it works)
+
+        returns: str name of the MCU, None if it doesn't work
+        """
+        # attempt to open the serial port
+        serial_interface: serial.Serial
+        try:
+            self.logger.info(f"asking {port} for its name")
+            serial_interface = serial.Serial(port, BAUD_RATE, timeout=1)
+
+            serial_interface.write(b"can_relay_mode,on\n")
+
+            for i in range(4):
+                self.logger.debug(f"attempt {i + 1} of 4 asking {port} for its name")
+                response = 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:
+                            self.logger.info(f"we are talking to {args[1]}")
+                            return args[1]
+                        break
+                except UnicodeDecodeError as e:
+                    self.logger.info(
+                        f"ignoring UnicodeDecodeError when asking for MCU name: {e}"
+                    )
+
+            if serial_interface.is_open:
+                # turn relay mode off if it failed to respond with its name
+                serial_interface.write(b"can_relay_mode,off\n")
+                serial_interface.close()
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException when asking for MCU name: {e}")
+
+        return None
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        try:
+            raw = str(self.serial_interface.readline(), "utf8")
+
+            if not raw:
+                return (None, None)
+
+            return (
+                self.string_to_viccan(raw, self.mcu_name),
+                raw,
+            )
+        except serial.SerialException as e:
+            self.logger.error(f"SerialException: {e}")
+            raise DeviceClosedException(f"serial port {self.port} closed unexpectedly")
+        except Exception:
+            return (None, None)  # pretty much no other error matters
+
+    def write(self, msg: VicCAN):
+        self.write_raw(viccan_to_string(msg))
+
+    def write_raw(self, msg: str):
+        self.serial_interface.write(bytes(msg, "utf8"))
+
+    def cleanup(self):
+        self.logger.info(f"closing serial port if open {self.port}")
+        try:
+            if self.serial_interface.is_open:
+                self.serial_interface.close()
+        except Exception as e:
+            self.logger.error(e)
+
+
+class CANConnector(Connector):
+    def __init__(self, logger: RcutilsLogger, clock: Clock, can_override: str):
+        super().__init__(logger, clock)
+
+        self.can_channel: str | None = None
+        self.can_bus: can.BusABC | None = None
+
+        avail = can.interfaces.socketcan.SocketcanBus._detect_available_configs()
+
+        if len(avail) == 0:
+            raise NoValidDeviceException("no CAN interfaces found")
+
+        # filter to busses whose channel matches the can_override
+        if can_override:
+            avail = list(
+                filter(
+                    lambda b: b.get("channel") == can_override,
+                    avail,
+                )
+            )
+
+        if (l := len(avail)) > 1:
+            channels = ", ".join(str(b.get("channel")) for b in avail)
+            raise MultipleValidDevicesException(
+                f"too many ({l}) CAN interfaces found: [{channels}]"
+            )
+
+        bus = avail[0]
+        self.can_channel = str(bus.get("channel"))
+        self.logger.info(f"found CAN interface '{self.can_channel}'")
+
+        try:
+            self.can_bus = can.Bus(
+                interface="socketcan",
+                channel=self.can_channel,
+                bitrate=1_000_000,
+            )
+        except can.CanError as e:
+            raise NoWorkingDeviceException(
+                f"could not open CAN channel '{self.can_channel}': {e}"
+            )
+
+        if self.can_channel and self.can_channel.startswith("v"):
+            self.logger.warn("likely using virtual CAN interface")
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        if not self.can_bus:
+            raise DeviceClosedException("CAN bus not initialized")
+
+        try:
+            message = self.can_bus.recv(timeout=0.0)
+        except can.CanError as e:
+            self.logger.error(f"CAN error while receiving: {e}")
+            raise DeviceClosedException("CAN bus closed unexpectedly")
+
+        if message is None:
+            return (None, None)
+
+        arbitration_id = message.arbitration_id & 0x7FF
+        data_bytes = bytes(message.data)
+
+        mcu_key = (arbitration_id >> 8) & 0b111
+        data_type_key = (arbitration_id >> 6) & 0b11
+        command = arbitration_id & 0x3F
+
+        key_to_mcu: dict[int, str] = {
+            1: "broadcast",
+            2: "core",
+            3: "arm",
+            4: "digit",
+            5: "faerie",
+            6: "citadel",
+        }
+
+        mcu_name = key_to_mcu.get(mcu_key)
+        if mcu_name is None:
+            self.logger.warn(
+                f"received CAN frame with unknown MCU key {mcu_key}; id=0x{arbitration_id:X}"
+            )
+            return (None, None)
+
+        data: list[float] = []
+
+        try:
+            if data_type_key == 3:
+                data = []
+            elif data_type_key == 0:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received double payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                (value,) = struct.unpack("<d", data_bytes[:8])
+                data = [float(value)]
+            elif data_type_key == 1:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received float32x2 payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                v1, v2 = struct.unpack("<ff", data_bytes[:8])
+                data = [float(v1), float(v2)]
+            elif data_type_key == 2:
+                if len(data_bytes) < 8:
+                    self.logger.warn(
+                        f"received int16x4 payload with insufficient length {len(data_bytes)}; dropping frame"
+                    )
+                    return (None, None)
+                i1, i2, i3, i4 = struct.unpack("<hhhh", data_bytes[:8])
+                data = [float(i1), float(i2), float(i3), float(i4)]
+            else:
+                self.logger.warn(
+                    f"received CAN frame with unknown data_type_key {data_type_key}; id=0x{arbitration_id:X}"
+                )
+                return (None, None)
+        except struct.error as e:
+            self.logger.error(f"error unpacking CAN payload: {e}")
+            return (None, None)
+
+        viccan = VicCAN(
+            mcu_name=mcu_name,
+            command_id=int(command),
+            data=data,
+        )
+
+        self.logger.debug(
+            f"received CAN frame id=0x{message.arbitration_id:X}, "
+            f"decoded as VicCAN(mcu_name={viccan.mcu_name}, command_id={viccan.command_id}, data={viccan.data})"
+        )
+
+        return (
+            viccan,
+            f"{viccan.mcu_name},{viccan.command_id},"
+            + ",".join(map(str, list(viccan.data))),
+        )
+
+    def write(self, msg: VicCAN):
+        if not self.can_bus:
+            raise DeviceClosedException("CAN bus not initialized")
+
+        # Build 11-bit arbitration ID according to the VicCAN scheme:
+        # bits 10..8: targeted MCU key
+        # bits 7..6:  data type key
+        # bits 5..0:  command
+
+        # Map MCU name to 3-bit key.
+        mcu_name = (msg.mcu_name or "").lower()
+        mcu_key_map: dict[str, int] = {
+            "broadcast": 1,
+            "core": 2,
+            "arm": 3,
+            "digit": 4,
+            "faerie": 5,
+            "citadel": 6,
+        }
+
+        if mcu_name not in mcu_key_map:
+            self.logger.error(
+                f"unknown VicCAN mcu_name '{msg.mcu_name}' for CAN frame; dropping message"
+            )
+            return
+
+        mcu_key = mcu_key_map[mcu_name] & 0b111
+
+        # Infer data type key from payload length according to the table:
+        # 0: double x1, 1: float32 x2, 2: int16 x4, 3: empty
+        data_len = len(msg.data)
+        if data_len == 0:
+            data_type_key = 3
+        elif data_len == 1:
+            data_type_key = 0
+        elif data_len == 2:
+            data_type_key = 1
+        elif data_len == 4:
+            data_type_key = 2
+        else:
+            # Fallback: treat any other non-zero length as float32 x2
+            self.logger.warn(
+                f"unexpected VicCAN data length {data_len}; encoding as float32 x2 (key=1) and truncating/padding as needed"
+            )
+            data_type_key = 1
+
+        # Command is limited to 6 bits.
+        command = int(msg.command_id)
+        if command < 0:
+            self.logger.error(f"invalid negative command_id for CAN frame: {command}")
+            return
+        if command > 0x3F:
+            self.logger.warn(
+                f"command_id 0x{command:X} exceeds 6-bit range; truncating to lower 6 bits"
+            )
+            command &= 0x3F
+
+        arbitration_id = (
+            ((mcu_key & 0b111) << 8) | ((data_type_key & 0b11) << 6) | (command & 0x3F)
+        )
+
+        # Map VicCAN.data (floats) to up to 8 CAN data bytes.
+        raw_bytes: list[int] = []
+        for value in msg.data:
+            try:
+                b = int(round(value))
+            except (TypeError, ValueError):
+                self.logger.error(
+                    f"non-numeric VicCAN data value: {value}; dropping message"
+                )
+                return
+
+            if b < 0 or b > 255:
+                self.logger.warn(
+                    f"VicCAN data value {value} out of byte range; clamping into [0, 255]"
+                )
+                b = max(0, min(255, b))
+
+            raw_bytes.append(b)
+
+        if len(raw_bytes) > 8:
+            self.logger.warn(
+                f"VicCAN data too long for single CAN frame ({len(raw_bytes)} > 8); truncating"
+            )
+            raw_bytes = raw_bytes[:8]
+
+        try:
+            can_message = can.Message(
+                arbitration_id=arbitration_id,
+                data=raw_bytes,
+                is_extended_id=False,
+            )
+        except Exception as e:
+            self.logger.error(f"failed to construct CAN message: {e}")
+            return
+
+        try:
+            self.can_bus.send(can_message)
+            self.logger.debug(
+                f"sent CAN frame id=0x{can_message.arbitration_id:X}, "
+                f"data={list(can_message.data)}"
+            )
+        except can.CanError as e:
+            self.logger.error(f"CAN error while sending: {e}")
+            raise DeviceClosedException("CAN bus closed unexpectedly")
+
+    def write_raw(self, msg: str):
+        self.logger.warn(f"write_raw is not supported for CANConnector. msg: {msg}")
+
+    def cleanup(self):
+        try:
+            if self.can_bus is not None:
+                self.logger.info("shutting down CAN bus")
+                self.can_bus.shutdown()
+        except Exception as e:
+            self.logger.error(e)
+
+
+class MockConnector(Connector):
+    def __init__(self, logger: RcutilsLogger, clock: Clock):
+        super().__init__(logger, clock)
+        # No hardware interface for MockConnector. Publish to `/anchor/from_vic/mock_mcu` instead.
+
+    def read(self) -> tuple[VicCAN | None, str | None]:
+        return (None, None)
+
+    def write(self, msg: VicCAN):
+        pass
+
+    def write_raw(self, msg: str):
+        pass

src/anchor_pkg/anchor_pkg/convert.py

@@ -0,0 +1,62 @@
+from astra_msgs.msg import VicCAN
+from std_msgs.msg import Header
+from builtin_interfaces.msg import Time
+from rclpy.impl.rcutils_logger import RcutilsLogger
+
+
+def string_to_viccan(
+    msg: str, mcu_name: str, logger: RcutilsLogger, time: Time
+) -> VicCAN | None:
+    """
+    Converts the serial string VicCAN format to a ROS2 VicCAN message.
+    Does not fill out the Header of the message.
+    On a failure, it will log at a debug level why it failed and return None.
+    """
+
+    parts: list[str] = msg.strip().split(",")
+
+    # don't need an extra check because len of .split output is always >= 1
+    if parts[0] != "can_relay_fromvic":
+        logger.debug(f"got non-CAN data from {mcu_name}: {msg}")
+        return None
+    elif len(parts) < 3:
+        logger.debug(f"got garbage (not enough parts) CAN data from {mcu_name}: {msg}")
+        return None
+    elif len(parts) > 7:
+        logger.debug(f"got garbage (too many parts) CAN data from {mcu_name}: {msg}")
+        return None
+
+    try:
+        command_id = int(parts[2])
+    except ValueError:
+        logger.debug(
+            f"got garbage (non-integer command id) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    if command_id not in range(64):
+        logger.debug(
+            f"got garbage (wrong command id {command_id}) CAN data from {mcu_name}: {msg}"
+        )
+        return None
+
+    try:
+        return VicCAN(
+            header=Header(
+                stamp=time,
+                frame_id="from_vic",
+            ),
+            mcu_name=parts[1],
+            command_id=command_id,
+            data=[float(x) for x in parts[3:]],
+        )
+    except ValueError:
+        logger.debug(f"got garbage (non-numerical) CAN data from {mcu_name}: {msg}")
+        return None
+
+
+def viccan_to_string(viccan: VicCAN) -> str:
+    """Converts a ROS2 VicCAN message to the serial string VicCAN format."""
+    # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max
+    data = "".join([f",{round(val,7)}" for val in viccan.data])
+    return f"can_relay_tovic,{viccan.mcu_name}{data}\n"

src/anchor_pkg/launch/rover.launch.py

@@ -0,0 +1,111 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    connector = LaunchConfiguration("connector")
+    serial_override = LaunchConfiguration("serial_override")
+    can_override = LaunchConfiguration("can_override")
+    use_ptz = LaunchConfiguration("use_ptz")
+
+    ld = LaunchDescription()
+
+    # arguments
+    ld.add_action(
+        DeclareLaunchArgument(
+            "connector",
+            default_value="auto",
+            description="Connector parameter for anchor node (default: 'auto')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "serial_override",
+            default_value="",
+            description="Serial port override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "can_override",
+            default_value="auto",
+            description="CAN network override parameter for anchor node (default: '')",
+        )
+    )
+
+    ld.add_action(
+        DeclareLaunchArgument(
+            "use_ptz",
+            default_value="true",  # must be string for launch system
+            description="Whether to launch PTZ node (default: true)",
+        )
+    )
+
+    # nodes
+    ld.add_action(
+        Node(
+            package="arm_pkg",
+            executable="arm",
+            name="arm",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="core",
+            name="core",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="core_pkg",
+            executable="ptz",
+            name="ptz",
+            output="both",
+            condition=IfCondition(use_ptz),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="bio_pkg",
+            executable="bio",
+            name="bio",
+            output="both",
+            parameters=[{"launch_mode": "anchor"}],
+            on_exit=Shutdown(),
+        )
+    )
+
+    ld.add_action(
+        Node(
+            package="anchor_pkg",
+            executable="anchor",
+            name="anchor",
+            output="both",
+            parameters=[
+                {
+                    "launch_mode": "anchor",
+                    "connector": connector,
+                    "serial_override": serial_override,
+                    "can_override": can_override,
+                }
+            ],
+            on_exit=Shutdown(),
+        )
+    )
+
+    return ld

src/anchor_pkg/package.xml

@@ -3,11 +3,16 @@
 <package format="3">
   <name>anchor_pkg</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="tristanmcginnis26@gmail.com">tristan</maintainer>
+  <description>Anchor -- ROS and CAN relay node</description>
+  <maintainer email="rjm0037@uah.edu">Riley</maintainer>
   <license>AGPL-3.0-only</license>
 
   <depend>rclpy</depend>
+  <depend>common_interfaces</depend>
+  <depend>python3-serial</depend>
+  <depend>python3-can</depend>
+
+  <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

@@ -1,287 +0,0 @@
-import rclpy
-from rclpy.node import Node
-
-import pygame
-
-import time
-
-import serial
-import sys
-import threading
-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
-
-
-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
-        super().__init__("arm_headless")
-
-        # Depricated, kept temporarily for reference
-        # self.create_timer(0.20, self.send_controls)#read and send controls
-
-        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)
-
-        # 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.laser_status = 0
-
-        # Initialize pygame
-        pygame.init()
-
-        # Initialize the gamepad module
-        pygame.joystick.init()
-
-        # Check if any gamepad is connected
-        if pygame.joystick.get_count() == 0:
-            print("No gamepad found.")
-            pygame.quit()
-            exit()
-        for event in pygame.event.get():
-            if event.type == pygame.QUIT:
-                pygame.quit()
-                exit()
-
-        # 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()}')
-        #
-        #
-
-
-    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
-
-                #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_manual()
-                        #self.read_feedback()
-                    self.gamepad = pygame.joystick.Joystick(0)
-                    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:
-                pygame.quit()
-                exit()
-        input = ArmManual()
-
-
-        # Triggers for gripper control
-        if self.gamepad.get_axis(2) > 0:#left trigger
-            input.gripper = -1
-        elif self.gamepad.get_axis(5) > 0:#right trigger
-            input.gripper = 1
-
-        # Toggle Laser
-        if self.gamepad.get_button(7):#Start
-            self.laser_status = 1
-        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
-
-            # Left stick X-axis for effector yaw
-            if self.gamepad.get_axis(0) > 0:
-                input.effector_yaw = 1
-            elif self.gamepad.get_axis(0) < 0:
-                input.effector_yaw = -1
-
-            # Right stick X-axis for effector roll
-            if self.gamepad.get_axis(3) > 0:
-                input.effector_roll = 1
-            elif self.gamepad.get_axis(3) < 0:
-                input.effector_roll = -1
-
-        else:   # Control arm axis
-            dpad_input = self.gamepad.get_hat(0)
-            input.axis0 = 0
-            if dpad_input[0] == 1:
-                input.axis0 = 1
-            elif dpad_input[0] == -1:
-                input.axis0 = -1
-
-            if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
-                input.axis1 = round(self.gamepad.get_axis(0))
-
-            if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15:
-                input.axis2 = -1 * round(self.gamepad.get_axis(1))
-
-            if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.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
-
-        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.manual_pub.publish(input)
-        else:
-            pass
-
-        pass
-        
-    # Depricated, kept temporarily for reference
-    # def send_controls(self):
-
-    #     for event in pygame.event.get():
-    #         if event.type == pygame.QUIT:
-    #             pygame.quit()
-    #             exit()
-    #     input = ControllerState()
-
-    #     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
-    #     else:
-    #         input.lb = False
-
-    #     #input.rb = self.gamepad.get_button(10)#Value must be converted to bool
-    #     if(self.gamepad.get_button(5)):#right bumper
-    #         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
-    #     else:
-    #         input.plus = False
-
-    #     #input.minus = self.gamepad.get_button(4)#minus button
-    #     if(self.gamepad.get_button(6)):#minus button
-    #         input.minus = True
-    #     else:
-    #         input.minus = False
-
-    #     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.a = self.gamepad.get_button(1)#A button
-    #     if(self.gamepad.get_button(0)):#A button
-    #         input.a = True
-    #     else:
-    #         input.a = False
-    #     #input.b = self.gamepad.get_button(0)#B button
-    #     if(self.gamepad.get_button(1)):#B button
-    #         input.b = True
-    #     else:
-    #         input.b = False
-    #     #input.x = self.gamepad.get_button(3)#X button
-    #     if(self.gamepad.get_button(2)):#X button
-    #         input.x = True
-    #     else:
-    #         input.x = False
-    #     #input.y = self.gamepad.get_button(2)#Y button
-    #     if(self.gamepad.get_button(3)):#Y button
-    #         input.y = True
-    #     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:
-    #         input.d_right = False
-    #     if(dpad_input[0] == -1):#D-pad left
-    #         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)
-    #     else:
-    #         pass
-
-
-
-        
-
-def main(args=None):
-    rclpy.init(args=args)
-
-    node = Headless()
-    
-    rclpy.spin(node)
-    rclpy.shutdown()
-
-    #tb_bs = BaseStation()
-    #node.run()
-
-
-if __name__ == '__main__':
-    main()

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,273 +1,256 @@
-import rclpy
-from rclpy.node import Node
-from rclpy import qos
-import serial
 import sys
 import threading
-import glob
-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
+import math
+from warnings import deprecated
 
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
+from rclpy import qos
 
-# 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 std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+from control_msgs.msg import JointJog
+from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual  # TODO: Old topics
+from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState
 
-from . import astra_arm
+control_qos = qos.QoSProfile(
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    depth=2,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
+    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),
+)
 
-# control_qos = qos.QoSProfile(
-#     history=qos.QoSHistoryPolicy.KEEP_LAST,
-#     depth=1,
-#     reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-#     durability=qos.QoSDurabilityPolicy.VOLATILE,
-#     deadline=1000,
-#     lifespan=500,
-#     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-#     liveliness_lease_duration=5000
-# )
-
-serial_pub = None
 thread = None
 
 
-class SerialRelay(Node):
+class ArmNode(Node):
+    """Relay between Anchor and Basestation/Headless/Moveit2 for Arm related topics."""
+
+    # Every non-fixed joint defined in Arm's URDF
+    # Used for JointState and JointJog messsages
+    all_joint_names = [
+        "axis_0_joint",
+        "axis_1_joint",
+        "axis_2_joint",
+        "axis_3_joint",
+        "wrist_yaw_joint",
+        "wrist_roll_joint",
+        "ef_gripper_left_joint",
+    ]
+
+    # Used to verify the length of an incoming VicCAN feedback message
+    # Key is VicCAN command_id, value is expected length of data list
+    viccan_socket_msg_len_dict = {
+        53: 4,
+        54: 4,
+        55: 4,
+        58: 4,
+        59: 4,
+    }
+
+    viccan_digit_msg_len_dict = {
+        54: 4,
+        55: 2,
+        59: 2,
+    }
+
     def __init__(self):
-        # Initialize 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.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
+        self.get_logger().info(f"arm launch_mode is: anchor")  # Hey I like the output
 
-        # 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.feedback_timer = self.create_timer(0.25, self.publish_feedback)
+        ##################################################
+        # Parameters
 
-        # 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.declare_parameter("use_old_topics", True)
+        self.use_old_topics = (
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
+        )
 
-        self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
+        ##################################################
+        # Old topics
 
-        # 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.use_old_topics:
+            # Anchor topics
+            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()
+            # Create publishers
+            self.socket_pub = self.create_publisher(
+                SocketFeedback, "/arm/feedback/socket", 10
+            )
+            self.arm_feedback = SocketFeedback()
+            self.digit_pub = self.create_publisher(
+                DigitFeedback, "/arm/feedback/digit", 10
+            )
+            self.digit_feedback = DigitFeedback()
+            self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
 
-        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        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 port in ports:
-                    try:
-                        # connect and send a ping command
-                        ser = serial.Serial(port, 115200, timeout=1)
-                        #print(f"Checking port {port}...")
-                        ser.write(b"ping\n")
-                        response = ser.read_until("\n")  # type: ignore
+            # Create subscribers
+            self.man_sub = self.create_subscription(
+                ArmManual, "/arm/control/manual", self.send_manual, 10
+            )
 
-                        # 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 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.")
-                time.sleep(1)
-                sys.exit(1)
-            
-            self.ser = serial.Serial(self.port, 115200)
-            atexit.register(self.cleanup)
+        ###################################################
+        # New topics
 
-    def run(self):
-        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 
+        # Anchor topics
 
-        try:
-            while rclpy.ok():
-                if self.launch_mode == 'arm':
-                    if self.ser.in_waiting:
-                        self.read_mcu()
-                    else:
-                        time.sleep(0.1)
-        except KeyboardInterrupt:
-            pass
-        finally:
-            self.cleanup()
+        # from_vic
+        self.anchor_fromvic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20
+        )
+        # to_vic
+        self.anchor_tovic_pub_ = self.create_publisher(
+            VicCAN, "/anchor/to_vic/relay", 20
+        )
 
+        # Control
 
-    #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}")
-                msg = String()
-                msg.data = output
-                self.debug_pub.publish(msg)
-        except serial.SerialException:
-            self.get_logger().info("SerialException caught... closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
-        except TypeError as e:
-            self.get_logger().info(f"TypeError: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
-        except Exception as e:
-            print(f"Exception: {e}")
-            print("Closing serial port.")
-            if self.ser.is_open:
-                self.ser.close()
-            pass
+        # Manual: /arm/manual/joint_jog is published by Basestation or Headless
+        self.man_jointjog_sub_ = self.create_subscription(
+            JointJog,
+            "/arm/manual/joint_jog",
+            self.jointjog_callback,
+            qos_profile=control_qos,
+        )
+        # IK: /joint_commands is published by JointTrajectoryController via topic_based_control
+        self.joint_command_sub_ = self.create_subscription(
+            JointState,
+            "/joint_commands",
+            self.joint_command_callback,
+            qos_profile=control_qos,
+        )
 
-    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
+        # Feedback
 
-        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)
+        # Combined Socket and Digit feedback
+        self.arm_feedback_pub_ = self.create_publisher(
+            ArmFeedback,
+            "/arm/feedback",
+            qos_profile=qos.qos_profile_sensor_data,
+        )
+        # IK arm pose: /joint_states is published from here to topic_based_control
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
+
+        ###################################################
+        # Saved state
+
+        # Combined Socket and Digit feedback
+        self.arm_feedback_new = ArmFeedback()
+
+        # IK Arm pose
+        self.saved_joint_state = JointState()
+        self.saved_joint_state.name = self.all_joint_names
+        # ... initialize with zeros
+        self.saved_joint_state.position = [0.0] * len(self.saved_joint_state.name)
+        self.saved_joint_state.velocity = [0.0] * len(self.saved_joint_state.name)
+
+    def jointjog_callback(self, msg: JointJog):
+        if len(msg.joint_names) != len(msg.velocities):
+            self.get_logger().debug("Ignoring malformed /arm/manual/joint_jog message.")
             return
 
-        # Debug output
-        tempMsg = String()
-        tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
-        #self.debug_pub.publish(tempMsg)
+        # Grab velocities from message
+        velocities = [
+            (
+                msg.velocities[msg.joint_names.index(joint_name)]  # type: ignore
+                if joint_name in msg.joint_names
+                else 0.0
+            )
+            for joint_name in self.all_joint_names
+        ]
+        # Deadzone
+        velocities = [vel if abs(vel) > 0.05 else 0.0 for vel in velocities]
 
-        # Target position is current position + input vector
-        current_position = self.arm.get_position_vector()
-        target_position = current_position + input_raw
+        self.send_velocities(velocities, msg.header)
 
+        # TODO: use msg.duration
 
-        #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}")
+    def joint_command_callback(self, msg: JointState):
+        if len(msg.position) < 7 and len(msg.velocity) < 7:
+            self.get_logger().debug("Ignoring malformed /joint_command message.")
+            return  # command needs either position or velocity for all 7 joints
 
-        # Debug output for current position
-        #tempMsg.data = "Current Position: " + str(current_position)
-        #self.debug_pub.publish(tempMsg)
+        # Grab velocities from message
+        velocities = [
+            (
+                msg.velocity[msg.name.index(joint_name)]  # type: ignore
+                if joint_name in msg.name
+                else 0.0
+            )
+            for joint_name in self.all_joint_names
+        ]
 
-        # Debug output for target position
-        #tempMsg.data = "Target Position: " + str(target_position)
-        #self.debug_pub.publish(tempMsg)
+        self.send_velocities(velocities, msg.header)
 
-        # 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 send_velocities(self, velocities: list[float], header: Header):
+        # ROS2's rad/s to VicCAN's deg/s*10; don't convert gripper's m/s
+        velocities = [
+            math.degrees(vel) * 10 if i < 6 else vel for i, vel in enumerate(velocities)
+        ]
 
+        # Send Axis 0-3
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="arm", command_id=43, data=velocities[0:3], header=header)
+        )
+        # Send Wrist yaw and roll
+        # TODO: Verify embedded
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="digit", command_id=43, data=velocities[4:5], header=header)
+        )
+        # Send End Effector Gripper
+        # TODO: Verify m/s received correctly by embedded
+        self.anchor_tovic_pub_.publish(
+            VicCAN(mcu_name="digit", command_id=26, data=[velocities[6]], header=header)
+        )
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
         axis1 = -1 * msg.axis1
         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
-            output = String()
-            output.data = msg
-            self.anchor_pub.publish(output)
-        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"))
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
+    def send_cmd(self, msg: str):
+        output = String(data=msg)
+        self.anchor_pub.publish(output)
+
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
-            #pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
-            #pass
             self.updateBusVoltage(output)
         elif output.startswith("can_relay_fromvic,arm,53"):
             self.updateMotorFeedback(output)
@@ -288,45 +271,149 @@ class SerialRelay(Node):
         else:
             return
 
+    def relay_fromvic(self, msg: VicCAN):
+        # Code for socket and digit are broken out for cleaner code
+        if msg.mcu_name == "arm":
+            self.process_fromvic_arm(msg)
+        elif msg.mcu_name == "digit":
+            self.process_fromvic_digit(msg)
+
+    def process_fromvic_arm(self, msg: VicCAN):
+        assert msg.mcu_name == "arm"
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in self.viccan_socket_msg_len_dict:
+            expected_len = self.viccan_socket_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:
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.temperature = float(msg.data[1]) / 10.0
+                    motor.voltage = float(msg.data[2]) / 10.0
+                    motor.current = float(msg.data[3]) / 10.0
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 54:  # Board voltages
+                self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.socket_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.socket_voltage.v5 = float(msg.data[2]) / 100.0
+                self.arm_feedback_new.socket_voltage.v3 = float(msg.data[3]) / 100.0
+                self.arm_feedback_new.socket_voltage.header.stamp = msg.header.stamp
+            case 55:  # Arm joint positions
+                angles = [angle / 10.0 for angle in msg.data]  # VicCAN sends deg*10
+                # Joint state publisher for URDF visualization
+                self.saved_joint_state.position[0] = math.radians(angles[0])  # Axis 0
+                self.saved_joint_state.position[1] = math.radians(angles[1])  # Axis 1
+                self.saved_joint_state.position[2] = math.radians(angles[2])  # Axis 2
+                self.saved_joint_state.position[3] = math.radians(angles[3])  # Axis 3
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+            case 58:  # REV SPARK MAX position and velocity feedback
+                motorId = round(msg.data[0])
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.arm_feedback_new.axis1_motor
+                    case 2:
+                        motor = self.arm_feedback_new.axis2_motor
+                    case 3:
+                        motor = self.arm_feedback_new.axis3_motor
+                    case 4:
+                        motor = self.arm_feedback_new.axis0_motor
+
+                if motor:
+                    motor.position = float(msg.data[1])
+                    motor.velocity = float(msg.data[2])
+                    motor.header.stamp = msg.header.stamp
+
+                self.arm_feedback_pub_.publish(self.arm_feedback_new)
+            case 59:  # Arm joint velocities
+                velocities = [vel / 100.0 for vel in msg.data]  # VicCAN sends deg/s*100
+                self.saved_joint_state.velocity[0] = math.radians(
+                    velocities[0]
+                )  # Axis 0
+                self.saved_joint_state.velocity[1] = math.radians(
+                    velocities[1]
+                )  # Axis 1
+                self.saved_joint_state.velocity[2] = math.radians(
+                    velocities[2]
+                )  # Axis 2
+                self.saved_joint_state.velocity[3] = math.radians(
+                    velocities[3]
+                )  # Axis 3
+                # Wrist is handled by digit feedback
+                self.saved_joint_state.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(self.saved_joint_state)
+
+    def process_fromvic_digit(self, msg: VicCAN):
+        assert msg.mcu_name == "digit"
+
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in self.viccan_digit_msg_len_dict:
+            expected_len = self.viccan_digit_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:
+            case 54:  # Board voltages
+                self.arm_feedback_new.digit_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0
+                self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0
+            case 55:  # Arm joint positions
+                self.saved_joint_state.position[4] = math.radians(
+                    msg.data[0]
+                )  # Wrist roll
+                self.saved_joint_state.position[5] = math.radians(
+                    msg.data[1]
+                )  # Wrist yaw
+
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def publish_feedback(self):
         self.socket_pub.publish(self.arm_feedback)
         self.digit_pub.publish(self.digit_feedback)
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateAngleFeedback(self, msg: str):
         # 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)
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateBusVoltage(self, msg: str):
         # Bus Voltage feedbacks
         parts = msg.split(",")
@@ -340,7 +427,8 @@ class SerialRelay(Node):
             self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
         else:
             self.get_logger().info("Invalid voltage feedback input format")
-    
+
+    @deprecated("Uses an old message type. Will be removed at some point.")
     def updateMotorFeedback(self, msg: str):
         parts = str(msg.strip()).split(",")
         motorId = round(float(parts[3]))
@@ -365,33 +453,27 @@ class SerialRelay(Node):
             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]*")
+def exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
 
-    def cleanup(self):
-        print("Cleaning up...")
-        try:
-            if self.ser.is_open:
-                self.ser.close()
-        except Exception as e:
-            exit(0)
-
-def myexcepthook(type, value, tb):
-    print("Uncaught exception:", type, value)
-    if serial_pub:
-        serial_pub.cleanup()
 
 def main(args=None):
     rclpy.init(args=args)
-    sys.excepthook = myexcepthook
 
-    global serial_pub
-    serial_pub = SerialRelay()
-    serial_pub.run()
+    # Catch termination signals and exit cleanly
+    signal.signal(signal.SIGTERM, exit_handler)
 
-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
+    arm_node = ArmNode()
+
+    try:
+        rclpy.spin(arm_node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        pass
+    finally:
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
     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

@@ -1,30 +1,22 @@
-from setuptools import find_packages, setup
-import os
-from glob import glob
+from setuptools import setup
 
-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=[package_name],
     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="David",
+    maintainer_email="ds0196@uah.edu",
+    description="Relays topics related to the arm between VicCAN (through Anchor) and basestation.",
+    license="AGPL-3.0-only",
     entry_points={
-        'console_scripts': [
-            'arm = arm_pkg.arm_node:main',
-            'headless = arm_pkg.arm_headless:main'
-        ],
+        "console_scripts": ["arm = arm_pkg.arm_node: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,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,  # Best Effort subscribers are still compatible with Reliable publishers
     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)
+    # 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,110 @@ class SerialRelay(Node):
         super().__init__("core_node")
 
         # Launch mode -- anchor vs core
-        self.declare_parameter('launch_mode', 'core')
-        self.launch_mode = self.get_parameter('launch_mode').value
+        self.declare_parameter("launch_mode", "core")
+        self.launch_mode = self.get_parameter("launch_mode").value
         self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
 
-
         ##################################################
         # Topics
 
         # Anchor
-        if self.launch_mode == 'anchor':
-            self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
-            self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
+        if self.launch_mode == "anchor":
+            self.anchor_fromvic_sub_ = self.create_subscription(
+                VicCAN, "/anchor/from_vic/core", self.relay_fromvic, 20
+            )
+            self.anchor_tovic_pub_ = self.create_publisher(
+                VicCAN, "/anchor/to_vic/relay", 20
+            )
 
-            self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+            self.anchor_sub = self.create_subscription(
+                String, "/anchor/core/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
         # Control
 
         # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        self.cmd_vel_sub_ = self.create_subscription(TwistStamped, '/cmd_vel', self.cmd_vel_callback, qos_profile=control_qos)
+        self.cmd_vel_sub_ = self.create_subscription(
+            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
+        )
         # manual twist -- [-1, 1] rather than real units
-        self.twist_man_sub_ = self.create_subscription(Twist, '/core/twist', self.twist_man_callback, qos_profile=control_qos)
+        self.twist_man_sub_ = self.create_subscription(
+            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+        )
         # manual flags -- brake mode and max duty cycle
-        self.control_state_sub_ = self.create_subscription(CoreCtrlState, '/core/control/state', self.control_state_callback, qos_profile=control_qos)
-        self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        self.control_state_sub_ = self.create_subscription(
+            CoreCtrlState,
+            "/core/control/state",
+            self.control_state_callback,
+            qos_profile=control_qos,
+        )
+        self.twist_max_duty = (
+            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+        )
 
         # Feedback
 
         # Consolidated and organized core feedback
-        self.feedback_new_pub_ = self.create_publisher(NewCoreFeedback, '/core/feedback_new', 10)
+        self.feedback_new_pub_ = self.create_publisher(
+            NewCoreFeedback,
+            "/core/feedback_new",
+            qos_profile=qos.qos_profile_sensor_data,
+        )
         self.feedback_new_state = NewCoreFeedback()
         self.feedback_new_state.fl_motor.id = 1
         self.feedback_new_state.bl_motor.id = 2
         self.feedback_new_state.fr_motor.id = 3
         self.feedback_new_state.br_motor.id = 4
-        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)  # TODO: not sure about this
+        self.telemetry_pub_timer = self.create_timer(
+            1.0, self.publish_feedback
+        )  # TODO: not sure about this
+        # Joint states for topic-based controller
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
         # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', 10)
+        self.imu_pub_ = self.create_publisher(
+            Imu, "/core/imu", qos_profile=qos.qos_profile_sensor_data
+        )
         self.imu_state = Imu()
         self.imu_state.header.frame_id = "core_bno055"
         # GPS (embedded u-blox M9N)
-        self.gps_pub_ = self.create_publisher(NavSatFix, '/gps/fix', 10)
+        self.gps_pub_ = self.create_publisher(
+            NavSatFix, "/gps/fix", qos_profile=qos.qos_profile_sensor_data
+        )
         self.gps_state = NavSatFix()
         self.gps_state.header.frame_id = "core_gps_antenna"
         self.gps_state.status.service = NavSatStatus.SERVICE_GPS
         self.gps_state.status.status = NavSatStatus.STATUS_NO_FIX
         self.gps_state.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
         # Barometer (embedded BMP-388)
-        self.baro_pub_ = self.create_publisher(Barometer, '/core/baro', 10)
+        self.baro_pub_ = self.create_publisher(
+            Barometer, "/core/baro", qos_profile=qos.qos_profile_sensor_data
+        )
         self.baro_state = Barometer()
         self.baro_state.header.frame_id = "core_bmp388"
 
         # Old
-        
+
         # /core/control
-        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
+        self.control_sub = self.create_subscription(
+            CoreControl, "/core/control", self.send_controls, 10
+        )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
         # /core/feedback
-        self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
+        self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
         self.core_feedback = CoreFeedback()
         # Debug
-        self.debug_pub = self.create_publisher(String, '/core/debug', 10)
-        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
-
+        self.debug_pub = self.create_publisher(String, "/core/debug", 10)
+        self.ping_service = self.create_service(
+            Empty, "/astra/core/ping", self.ping_callback
+        )
 
         ##################################################
         # Find microcontroller (Non-anchor only)
 
         # Core (non-anchor) specific
-        if self.launch_mode == 'core':
+        if self.launch_mode == "core":
             # Loop through all serial devices on the computer to check for the MCU
             self.port = None
             ports = SerialRelay.list_serial_ports()
@@ -125,7 +174,7 @@ class SerialRelay(Node):
                     try:
                         # connect and send a ping command
                         ser = serial.Serial(port, 115200, timeout=1)
-                        #(f"Checking port {port}...")
+                        # (f"Checking port {port}...")
                         ser.write(b"ping\n")
                         response = ser.read_until("\n")  # type: ignore
 
@@ -140,17 +189,16 @@ class SerialRelay(Node):
                         pass
                 if self.port is not None:
                     break
-        
+
             if self.port is None:
                 self.get_logger().info("Unable to find MCU...")
                 time.sleep(1)
                 sys.exit(1)
-        
+
             self.ser = serial.Serial(self.port, 115200)
             atexit.register(self.cleanup)
         # end __init__()
 
-
     def run(self):
         # This thread makes all the update processes run in the background
         global thread
@@ -159,15 +207,15 @@ class SerialRelay(Node):
 
         try:
             while rclpy.ok():
-                if self.launch_mode == 'core':
-                    self.read_MCU() # Check the MCU for updates
+                if self.launch_mode == "core":
+                    self.read_MCU()  # Check the MCU for updates
         except KeyboardInterrupt:
             sys.exit(0)
 
     def read_MCU(self):  # NON-ANCHOR SPECIFIC
         try:
             output = str(self.ser.readline(), "utf8")
-            
+
             if output:
                 # All output over debug temporarily
                 print(f"[MCU] {output}")
@@ -197,8 +245,8 @@ class SerialRelay(Node):
     def scale_duty(self, value: float, max_speed: float):
         leftMin = -1
         leftMax = 1
-        rightMin = -max_speed/100.0
-        rightMax = max_speed/100.0
+        rightMin = -max_speed / 100.0
+        rightMax = max_speed / 100.0
 
         # Figure out how 'wide' each range is
         leftSpan = leftMax - leftMin
@@ -211,17 +259,29 @@ class SerialRelay(Node):
         return str(rightMin + (valueScaled * rightSpan))
 
     def send_controls(self, msg: CoreControl):
-        if(msg.turn_to_enable):
-            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
+        if msg.turn_to_enable:
+            command = (
+                "can_relay_tovic,core,41,"
+                + str(msg.turn_to)
+                + ","
+                + str(msg.turn_to_timeout)
+                + "\n"
+            )
         else:
-            command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
+            command = (
+                "can_relay_tovic,core,19,"
+                + self.scale_duty(msg.left_stick, msg.max_speed)
+                + ","
+                + self.scale_duty(msg.right_stick, msg.max_speed)
+                + "\n"
+            )
         self.send_cmd(command)
 
         # Brake mode
-        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n'
+        command = "can_relay_tovic,core,18," + str(int(msg.brake)) + "\n"
         self.send_cmd(command)
 
-        #print(f"[Sys] Relaying: {command}")
+        # print(f"[Sys] Relaying: {command}")
 
     def cmd_vel_callback(self, msg: TwistStamped):
         linear = msg.twist.linear.x
@@ -232,20 +292,22 @@ class SerialRelay(Node):
 
         vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
         vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        
+
         self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
 
     def twist_man_callback(self, msg: Twist):
         linear = msg.linear.x  # [-1 1] for forward/back from left stick y
         angular = msg.angular.z  # [-1 1] for left/right from right stick x
 
-        if (linear < 0):  # reverse turning direction when going backwards (WIP)
+        if linear < 0:  # reverse turning direction when going backwards (WIP)
             angular *= -1
 
         if abs(linear) > 1 or abs(angular) > 1:
             # if speed is greater than 1, then there is a problem
             # make it look like a problem and don't just run away lmao
-            linear = copysign(0.25, linear)  # 0.25 duty cycle in direction of control (hopefully slow)
+            linear = copysign(
+                0.25, linear
+            )  # 0.25 duty cycle in direction of control (hopefully slow)
             angular = copysign(0.25, angular)
 
         duty_left = linear - angular
@@ -256,8 +318,12 @@ class SerialRelay(Node):
 
         # Apply max duty cycle
         # Joysticks provide values [-1, 1] rather than real units
-        duty_left = map_range(duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty)
-        duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
+        duty_left = map_range(
+            duty_left, -1, 1, -self.twist_max_duty, self.twist_max_duty
+        )
+        duty_right = map_range(
+            duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty
+        )
 
         self.send_viccan(19, [duty_left, duty_right])
 
@@ -269,24 +335,24 @@ class SerialRelay(Node):
         self.twist_max_duty = msg.max_duty  # twist_man_callback will handle this
 
     def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor':
-            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()#Convert to std_msg string
+        if self.launch_mode == "anchor":
+            # self.get_logger().info(f"[Core to Anchor Relay] {msg}")
+            output = String()  # Convert to std_msg string
             output.data = msg
             self.anchor_pub.publish(output)
-        elif self.launch_mode == 'core':
+        elif self.launch_mode == "core":
             self.get_logger().info(f"[Core to MCU] {msg}")
             self.ser.write(bytes(msg, "utf8"))
 
-
     def send_viccan(self, cmd_id: int, data: list[float]):
-        self.anchor_tovic_pub_.publish(VicCAN(
-            header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
-            mcu_name="core",
-            command_id=cmd_id,
-            data=data
-        ))
-
+        self.anchor_tovic_pub_.publish(
+            VicCAN(
+                header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"),
+                mcu_name="core",
+                command_id=cmd_id,
+                data=data,
+            )
+        )
 
     def anchor_feedback(self, msg: String):
         output = msg.data
@@ -350,121 +416,150 @@ class SerialRelay(Node):
             return
         self.feedback_new_state.header.stamp = self.get_clock().now().to_msg()
         self.feedback_new_pub_.publish(self.feedback_new_state)
-        #self.get_logger().info(f"[Core Anchor] {msg}")
+        # self.get_logger().info(f"[Core Anchor] {msg}")
 
     def relay_fromvic(self, msg: VicCAN):
         # Assume that the message is coming from Core
         # skill diff if not
-        
-        # TODO: add len(msg.data) checks to each feedback message
 
-        # GNSS
-        if msg.command_id == 48:  # GNSS Latitude
-            self.gps_state.latitude = float(msg.data[0])
-        elif msg.command_id == 49:  # GNSS Longitude
-            self.gps_state.longitude = float(msg.data[0])
-        elif msg.command_id == 50:  # GNSS Satellite count and altitude
-            self.gps_state.status.status = NavSatStatus.STATUS_FIX if int(msg.data[0]) >= 3 else NavSatStatus.STATUS_NO_FIX
-            self.gps_state.altitude = float(msg.data[1])
-            self.gps_state.header.stamp = msg.header.stamp
-            self.gps_pub_.publish(self.gps_state)
-        # IMU
-        elif msg.command_id == 51:  # Gyro x, y, z, and imu calibration
-            self.feedback_new_state.imu_calib = round(float(msg.data[3]))
-            self.imu_state.angular_velocity.x = float(msg.data[0])
-            self.imu_state.angular_velocity.y = float(msg.data[1])
-            self.imu_state.angular_velocity.z = float(msg.data[2])
-            self.imu_state.header.stamp = msg.header.stamp
-        elif msg.command_id == 52:  # Accel x, y, z, heading
-            self.imu_state.linear_acceleration.x = float(msg.data[0])
-            self.imu_state.linear_acceleration.y = float(msg.data[1])
-            self.imu_state.linear_acceleration.z = float(msg.data[2])
-            # Deal with quaternion
-            r = Rotation.from_euler('z', float(msg.data[3]), degrees=True)
-            q = r.as_quat()
-            self.imu_state.orientation.x = q[0]
-            self.imu_state.orientation.y = q[1]
-            self.imu_state.orientation.z = q[2]
-            self.imu_state.orientation.w = q[3]
-            self.imu_state.header.stamp = msg.header.stamp
-            self.imu_pub_.publish(self.imu_state)
-        # REV Motors
-        elif msg.command_id == 53:  # REV SPARK MAX feedback
-            motorId = round(float(msg.data[0]))
-            temp = float(msg.data[1]) / 10.0
-            voltage = float(msg.data[2]) / 10.0
-            current = float(msg.data[3]) / 10.0
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.temperature = temp
-                self.feedback_new_state.fl_motor.voltage = voltage
-                self.feedback_new_state.fl_motor.current = current
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.temperature = temp
-                self.feedback_new_state.bl_motor.voltage = voltage
-                self.feedback_new_state.bl_motor.current = current
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.temperature = temp
-                self.feedback_new_state.fr_motor.voltage = voltage
-                self.feedback_new_state.fr_motor.current = current
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.temperature = temp
-                self.feedback_new_state.br_motor.voltage = voltage
-                self.feedback_new_state.br_motor.current = current
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-            self.feedback_new_pub_.publish(self.feedback_new_state)
-        # Board voltage
-        elif msg.command_id == 54:  # Voltages batt, 12, 5, 3, all * 100
-            self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
-            self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
-            self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
-            self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
-        # Baro
-        elif msg.command_id == 56:  # BMP temperature, altitude, pressure
-            self.baro_state.temperature = float(msg.data[0])
-            self.baro_state.altitude = float(msg.data[1])
-            self.baro_state.pressure = float(msg.data[2])
-            self.baro_state.header.stamp = msg.header.stamp
-            self.baro_pub_.publish(self.baro_state)
-        # REV Motors (pos and vel)
-        elif msg.command_id == 58:  # REV position and velocity
-            motorId = round(float(msg.data[0]))
-            position = float(msg.data[1])
-            velocity = float(msg.data[2])
-            if motorId == 1:
-                self.feedback_new_state.fl_motor.position = position
-                self.feedback_new_state.fl_motor.velocity = velocity
-                self.feedback_new_state.fl_motor.header.stamp = msg.header.stamp
-            elif motorId == 2:
-                self.feedback_new_state.bl_motor.position = position
-                self.feedback_new_state.bl_motor.velocity = velocity
-                self.feedback_new_state.bl_motor.header.stamp = msg.header.stamp
-            elif motorId == 3:
-                self.feedback_new_state.fr_motor.position = position
-                self.feedback_new_state.fr_motor.velocity = velocity
-                self.feedback_new_state.fr_motor.header.stamp = msg.header.stamp
-            elif motorId == 4:
-                self.feedback_new_state.br_motor.position = position
-                self.feedback_new_state.br_motor.velocity = velocity
-                self.feedback_new_state.br_motor.header.stamp = msg.header.stamp
-        else:
-            return
+        # Check message len to prevent crashing on bad data
+        if msg.command_id in viccan_msg_len_dict:
+            expected_len = viccan_msg_len_dict[msg.command_id]
+            if len(msg.data) != expected_len:
+                self.get_logger().warning(
+                    f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
+                )
+                return
 
+        match msg.command_id:
+            # GNSS
+            case 48:  # GNSS Latitude
+                self.gps_state.latitude = float(msg.data[0])
+            case 49:  # GNSS Longitude
+                self.gps_state.longitude = float(msg.data[0])
+            case 50:  # GNSS Satellite count and altitude
+                self.gps_state.status.status = (
+                    NavSatStatus.STATUS_FIX
+                    if int(msg.data[0]) >= 3
+                    else NavSatStatus.STATUS_NO_FIX
+                )
+                self.gps_state.altitude = float(msg.data[1])
+                self.gps_state.header.stamp = msg.header.stamp
+                self.gps_pub_.publish(self.gps_state)
+            # IMU
+            case 51:  # Gyro x, y, z, and imu calibration
+                self.feedback_new_state.imu_calib = round(float(msg.data[3]))
+                self.imu_state.angular_velocity.x = float(msg.data[0])
+                self.imu_state.angular_velocity.y = float(msg.data[1])
+                self.imu_state.angular_velocity.z = float(msg.data[2])
+                self.imu_state.header.stamp = msg.header.stamp
+            case 52:  # Accel x, y, z, heading
+                self.imu_state.linear_acceleration.x = float(msg.data[0])
+                self.imu_state.linear_acceleration.y = float(msg.data[1])
+                self.imu_state.linear_acceleration.z = float(msg.data[2])
+                # Deal with quaternion
+                r = Rotation.from_euler("z", float(msg.data[3]), degrees=True)
+                q = r.as_quat()
+                self.imu_state.orientation.x = q[0]
+                self.imu_state.orientation.y = q[1]
+                self.imu_state.orientation.z = q[2]
+                self.imu_state.orientation.w = q[3]
+                self.imu_state.header.stamp = msg.header.stamp
+                self.imu_pub_.publish(self.imu_state)
+            # REV Motors
+            case 53:  # REV SPARK MAX feedback
+                motorId = round(float(msg.data[0]))
+                temp = float(msg.data[1]) / 10.0
+                voltage = float(msg.data[2]) / 10.0
+                current = float(msg.data[3]) / 10.0
+                motor: RevMotorState | None = None
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                    case _:
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 53 with invalid motorId {motorId}"
+                        )
+                        return
+
+                if motor:
+                    motor.temperature = temp
+                    motor.voltage = voltage
+                    motor.current = current
+                    motor.header.stamp = msg.header.stamp
+
+                self.feedback_new_pub_.publish(self.feedback_new_state)
+            # Board voltage
+            case 54:  # Voltages batt, 12, 5, 3, all * 100
+                self.feedback_new_state.board_voltage.vbatt = float(msg.data[0]) / 100.0
+                self.feedback_new_state.board_voltage.v12 = float(msg.data[1]) / 100.0
+                self.feedback_new_state.board_voltage.v5 = float(msg.data[2]) / 100.0
+                self.feedback_new_state.board_voltage.v3 = float(msg.data[3]) / 100.0
+            # Baro
+            case 56:  # BMP temperature, altitude, pressure
+                self.baro_state.temperature = float(msg.data[0])
+                self.baro_state.altitude = float(msg.data[1])
+                self.baro_state.pressure = float(msg.data[2])
+                self.baro_state.header.stamp = msg.header.stamp
+                self.baro_pub_.publish(self.baro_state)
+            # REV Motors (pos and vel)
+            case 58:  # REV position and velocity
+                motorId = round(float(msg.data[0]))
+                position = float(msg.data[1])
+                velocity = float(msg.data[2])
+                joint_state_msg = (
+                    JointState()
+                )  # TODO: not sure if all motors should be in each message or not
+                joint_state_msg.position = [
+                    position * (2 * pi) / CORE_GEAR_RATIO
+                ]  # revolutions to radians
+                joint_state_msg.velocity = [
+                    velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
+                ]  # RPM to rad/s
+
+                motor: RevMotorState | None = None
+
+                match motorId:
+                    case 1:
+                        motor = self.feedback_new_state.fl_motor
+                        joint_state_msg.name = ["fl_motor_joint"]
+                    case 2:
+                        motor = self.feedback_new_state.bl_motor
+                        joint_state_msg.name = ["bl_motor_joint"]
+                    case 3:
+                        motor = self.feedback_new_state.fr_motor
+                        joint_state_msg.name = ["fr_motor_joint"]
+                    case 4:
+                        motor = self.feedback_new_state.br_motor
+                        joint_state_msg.name = ["br_motor_joint"]
+                    case _:
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
+                        )
+                        return
+
+                joint_state_msg.header.stamp = msg.header.stamp
+                self.joint_state_pub_.publish(joint_state_msg)
+            case _:
+                return
 
     def publish_feedback(self):
-        #self.get_logger().info(f"[Core] {self.core_feedback}")
+        # self.get_logger().info(f"[Core] {self.core_feedback}")
         self.feedback_pub.publish(self.core_feedback)
 
     def ping_callback(self, request, response):
         return response
 
-
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-    
+
     def cleanup(self):
         print("Cleaning up before terminating...")
         try:
@@ -479,20 +574,26 @@ def myexcepthook(type, value, tb):
     if serial_pub:
         serial_pub.cleanup()
 
-def map_range(value: float, in_min: float, in_max: float, out_min: float, out_max: float):
+
+def map_range(
+    value: float, in_min: float, in_max: float, out_min: float, out_max: float
+):
     return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
 def main(args=None):
-   rclpy.init(args=args)
-   sys.excepthook = myexcepthook
+    rclpy.init(args=args)
+    sys.excepthook = myexcepthook
 
-   global serial_pub
+    global serial_pub
 
-   serial_pub = SerialRelay()
-   serial_pub.run()
+    serial_pub = SerialRelay()
+    serial_pub.run()
 
-if __name__ == '__main__':
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+
+if __name__ == "__main__":
+    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

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,53 +1,91 @@
 import rclpy
 from rclpy.node import Node
+from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
 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 std_srvs.srv import Trigger
+from std_msgs.msg import Header
+from geometry_msgs.msg import Twist, TwistStamped
+from control_msgs.msg import JointJog
+from astra_msgs.msg import CoreControl, ArmManual, BioControl
+from astra_msgs.msg import CoreCtrlState
+
+import warnings
+
+# Literally headless
+warnings.filterwarnings(
+    "ignore",
+    message="Your system is avx2 capable but pygame was not built with support for it.",
+)
 
 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,
     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)
+    # 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"
+
+STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05"))
+ARM_DEADZONE = float(os.getenv("ARM_DEADZONE", "0.2"))
 
 
 class Headless(Node):
+    # Every non-fixed joint defined in Arm's URDF
+    # Used for JointState and JointJog messsages
+    all_joint_names = [
+        "axis_0_joint",
+        "axis_1_joint",
+        "axis_2_joint",
+        "axis_3_joint",
+        "wrist_yaw_joint",
+        "wrist_roll_joint",
+        "ef_gripper_left_joint",
+    ]
+
     def __init__(self):
         # Initialize pygame first
         pygame.init()
         pygame.joystick.init()
+        super().__init__("headless_node")
+
+        ##################################################
+        # Preamble
+
+        # 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,62 +99,168 @@ 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)
 
-        # Now initialize the ROS2 node
-        super().__init__("headless")
-        self.create_timer(0.15, self.send_controls)
+            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()}")
 
-        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)
+            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
 
-        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)
+        ##################################################
+        # Parameters
 
+        self.declare_parameter("use_old_topics", True)
+        self.use_old_topics = (
+            self.get_parameter("use_old_topics").get_parameter_value().bool_value
+        )
+
+        self.declare_parameter("use_bio", False)
+        self.use_bio = self.get_parameter("use_bio").get_parameter_value().bool_value
+
+        self.declare_parameter("use_arm_ik", False)
+        self.use_arm_ik = (
+            self.get_parameter("use_arm_ik").get_parameter_value().bool_value
+        )
+
+        # NOTE: only applicable if use_old_topics == True
+        self.declare_parameter("use_new_arm_manual_scheme", True)
+        self.use_new_arm_manual_scheme = (
+            self.get_parameter("use_new_arm_manual_scheme")
+            .get_parameter_value()
+            .bool_value
+        )
+
+        # Check parameter validity
+        if self.use_arm_ik and self.use_old_topics:
+            self.get_logger().fatal("Old topics do not support arm IK control.")
+            sys.exit(1)
+        if not self.use_new_arm_manual_scheme and not self.use_old_topics:
+            self.get_logger().warn(
+                f"New arm manual does not support old control scheme. Defaulting to new scheme."
+            )
 
         self.ctrl_mode = "core"  # Start in core mode
         self.core_brake_mode = False
         self.core_max_duty = 0.5  # Default max duty cycle (walking speed)
 
+        ##################################################
+        # Old Topics
+
+        if self.use_old_topics:
+            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)
+
+        ##################################################
+        # New Topics
+
+        if not self.use_old_topics:
+            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.arm_manual_pub_ = self.create_publisher(
+                JointJog, "/arm/manual_new", qos_profile=control_qos
+            )
+
+            self.arm_ik_twist_publisher = self.create_publisher(
+                TwistStamped, "/servo_node/delta_twist_cmds", qos_profile=control_qos
+            )
+            self.arm_ik_jointjog_publisher = self.create_publisher(
+                JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos
+            )
+
+            # TODO: add new bio topics
+
+        ##################################################
+        # Timers
+
+        self.create_timer(0.1, self.send_controls)
+
+        ##################################################
+        # Services
+
+        # If using IK control, we have to "start" the servo node to enable it to accept commands
+        self.servo_start_client = None
+        if self.use_arm_ik:
+            self.get_logger().info("Starting servo node for IK control...")
+            self.servo_start_client = self.create_client(
+                Trigger, "/servo_node/start_servo"
+            )
+            timeout_counter = 0
+            while not self.servo_start_client.wait_for_service(timeout_sec=1.0):
+                self.get_logger().info("Waiting for servo_node/start_servo service...")
+                timeout_counter += 1
+                if timeout_counter >= 10:
+                    self.get_logger().error(
+                        "Servo's start service not available. IK control will not work."
+                    )
+                    break
+            if self.servo_start_client.service_is_ready():
+                self.servo_start_client.call_async(Trigger.Request())
+
         # 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()
-                time.sleep(0.1)  # Small delay to avoid CPU hogging
-        except KeyboardInterrupt:
-            sys.exit(0)
+    def stop_all(self):
+        if self.use_old_topics:
+            self.core_publisher.publish(CORE_STOP_MSG)
+            self.arm_publisher.publish(ARM_STOP_MSG)
+            self.bio_publisher.publish(BIO_STOP_MSG)
+        else:
+            self.core_twist_pub_.publish(CORE_STOP_TWIST_MSG)
+            if self.use_arm_ik:
+                self.arm_ik_twist_publisher.publish(self.arm_ik_twist_stop_msg())
+            else:
+                self.arm_manual_pub_.publish(self.arm_manual_stop_msg())
+            # TODO: add bio here after implementing new topics
 
     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)
-            self.arm_publisher.publish(ARM_STOP_MSG)
-            self.bio_publisher.publish(BIO_STOP_MSG)
+            # Stop the rover if controller disconnected
+            self.stop_all()
             self.get_logger().info("Final stop commands sent. Shutting down.")
             # Clean up
             pygame.quit()
             sys.exit(0)
 
-
         new_ctrl_mode = self.ctrl_mode  # if "" then inequality will always be true
 
         # Check for control mode change
@@ -127,22 +271,51 @@ class Headless(Node):
             new_ctrl_mode = "core"
 
         if new_ctrl_mode != self.ctrl_mode:
+            self.stop_all()
             self.gamepad.rumble(0.6, 0.7, 75)
             self.ctrl_mode = new_ctrl_mode
             self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
+            if self.ctrl_mode == "arm" and self.use_bio:
+                self.get_logger().warning("NOTE: Using bio instead of arm.")
 
+        # Actually send the controls
+        if self.ctrl_mode == "core":
+            self.send_core()
+            if self.use_old_topics:
+                if self.use_bio:
+                    self.bio_publisher.publish(BIO_STOP_MSG)
+                else:
+                    self.arm_publisher.publish(ARM_STOP_MSG)
+            # New topics shouldn't need to constantly send zeroes imo
+        else:
+            if self.use_bio:
+                self.send_bio()
+            else:
+                self.send_arm()
+            if self.use_old_topics:
+                self.core_publisher.publish(CORE_STOP_MSG)
+            # Ditto
 
-        # CORE
-        if self.ctrl_mode == "core" and CORE_MODE == "duty":
+    def send_core(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
+
+        if self.use_old_topics:
             input = CoreControl()
             input.max_speed = 90
 
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            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,34 +325,26 @@ 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()
 
-            # Collect controller state
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            button_a = self.gamepad.get_button(0)
-            left_bumper = self.gamepad.get_button(4)
-            right_bumper = self.gamepad.get_button(5)
+        else:  # New topics
+            input = Twist()
 
             # 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,96 +357,357 @@ 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}"
+                )
 
+    def send_arm(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
 
-        # ARM and BIO
-        if self.ctrl_mode == "arm":
+        # OLD MANUAL
+        # ==========
+
+        if not self.use_arm_ik and self.use_old_topics:
             arm_input = ArmManual()
-            
-            # Collect controller state
-            left_stick_x = deadzone(self.gamepad.get_axis(0))
-            left_stick_y = deadzone(self.gamepad.get_axis(1))
-            left_trigger = deadzone(self.gamepad.get_axis(2))
-            right_stick_x = deadzone(self.gamepad.get_axis(3))
-            right_stick_y = deadzone(self.gamepad.get_axis(4))
-            right_trigger = deadzone(self.gamepad.get_axis(5))
-            right_bumper = self.gamepad.get_button(5)
-            dpad_input = self.gamepad.get_hat(0)
 
+            # OLD ARM MANUAL CONTROL SCHEME
+            if not self.use_new_arm_manual_scheme:
+                # EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
 
-            # EF Grippers
-            if left_trigger > 0 and right_trigger > 0:
-                arm_input.gripper = 0
-            elif left_trigger > 0:
-                arm_input.gripper = -1
-            elif right_trigger > 0:
-                arm_input.gripper = 1
+                # Axis 0
+                if dpad_input[0] == 1:
+                    arm_input.axis0 = 1
+                elif dpad_input[0] == -1:
+                    arm_input.axis0 = -1
 
-            # Axis 0
-            if dpad_input[0] == 1:
-                arm_input.axis0 = 1
-            elif dpad_input[0] == -1:
-                arm_input.axis0 = -1
+                if right_bumper:  # Control end effector
 
+                    # Effector yaw
+                    if left_stick_x > 0:
+                        arm_input.effector_yaw = 1
+                    elif left_stick_x < 0:
+                        arm_input.effector_yaw = -1
 
-            if right_bumper:  # Control end effector
+                    # Effector roll
+                    if right_stick_x > 0:
+                        arm_input.effector_roll = 1
+                    elif right_stick_x < 0:
+                        arm_input.effector_roll = -1
 
-                # Effector yaw
-                if left_stick_x > 0:
-                    arm_input.effector_yaw = 1
-                elif left_stick_x < 0:
-                    arm_input.effector_yaw = -1
+                else:  # Control arm axis
 
-                # Effector roll
-                if right_stick_x > 0:
-                    arm_input.effector_roll = 1
-                elif right_stick_x < 0:
+                    # Axis 1
+                    if abs(left_stick_x) > 0.15:
+                        arm_input.axis1 = round(left_stick_x)
+
+                    # Axis 2
+                    if abs(left_stick_y) > 0.15:
+                        arm_input.axis2 = -1 * round(left_stick_y)
+
+                    # Axis 3
+                    if abs(right_stick_y) > 0.15:
+                        arm_input.axis3 = -1 * round(right_stick_y)
+
+            # NEW ARM MANUAL CONTROL SCHEME
+            if self.use_new_arm_manual_scheme:
+                # Right stick: EF yaw and axis 3
+                # Left stick: axis 1 and 2
+                # D-pad: axis 0 and _
+                # Triggers: EF grippers
+                # Bumpers: EF roll
+                # A: brake
+                # B: linear actuator in
+                # X: _
+                # Y: linear actuator out
+
+                # Right stick: EF yaw and axis 3
+                arm_input.effector_yaw = stick_to_arm_direction(right_stick_x)
+                arm_input.axis3 = -1 * stick_to_arm_direction(right_stick_y)
+
+                # Left stick: axis 1 and 2
+                arm_input.axis1 = stick_to_arm_direction(left_stick_x)
+                arm_input.axis2 = -1 * stick_to_arm_direction(left_stick_y)
+
+                # D-pad: axis 0 and _
+                arm_input.axis0 = int(dpad_input[0])
+
+                # Triggers: EF Grippers
+                if left_trigger > 0 and right_trigger > 0:
+                    arm_input.gripper = 0
+                elif left_trigger > 0:
+                    arm_input.gripper = -1
+                elif right_trigger > 0:
+                    arm_input.gripper = 1
+
+                # Bumpers: EF roll
+                if left_bumper > 0 and right_bumper > 0:
+                    arm_input.effector_roll = 0
+                elif left_bumper > 0:
                     arm_input.effector_roll = -1
+                elif right_bumper > 0:
+                    arm_input.effector_roll = 1
 
-            else:   # Control arm axis
+                # A: brake
+                if button_a:
+                    arm_input.brake = True
 
-                # Axis 1
-                if abs(left_stick_x) > .15:
-                    arm_input.axis1 = round(left_stick_x)
+                # Y: linear actuator
+                if button_y and not button_b:
+                    arm_input.linear_actuator = 1
+                elif button_b and not button_y:
+                    arm_input.linear_actuator = -1
+                else:
+                    arm_input.linear_actuator = 0
 
-                # Axis 2
-                if abs(left_stick_y) > .15:
-                    arm_input.axis2 = -1 * round(left_stick_y)
-
-                # Axis 3
-                if abs(right_stick_y) > .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))
-
-            self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(arm_input)
-            # self.bio_publisher.publish(bio_input)
+
+        # NEW MANUAL
+        # ==========
+
+        elif not self.use_arm_ik and not self.use_old_topics:
+            arm_input = JointJog()
+            arm_input.header.frame_id = "base_link"
+            arm_input.header.stamp = self.get_clock().now().to_msg()
+            arm_input.joint_names = self.all_joint_names
+            arm_input.velocities = [0.0] * len(self.all_joint_names)
+
+            # Right stick: EF yaw and axis 3
+            # Left stick: axis 1 and 2
+            # D-pad: axis 0 and _
+            # Triggers: EF grippers
+            # Bumpers: EF roll
+            # A: brake
+            # B: linear actuator in
+            # X: _
+            # Y: linear actuator out
+
+            # Right stick: EF yaw and axis 3
+            arm_input.velocities[self.all_joint_names.index("wrist_yaw_joint")] = float(
+                stick_to_arm_direction(right_stick_x)
+            )
+            arm_input.velocities[self.all_joint_names.index("axis_3_joint")] = float(
+                stick_to_arm_direction(right_stick_y)
+            )
+
+            # Left stick: axis 1 and 2
+            arm_input.velocities[self.all_joint_names.index("axis_1_joint")] = float(
+                stick_to_arm_direction(left_stick_x)
+            )
+            arm_input.velocities[self.all_joint_names.index("axis_2_joint")] = float(
+                stick_to_arm_direction(left_stick_y)
+            )
+
+            # D-pad: axis 0 and _
+            arm_input.velocities[self.all_joint_names.index("axis_0_joint")] = float(
+                dpad_input[0]
+            )
+
+            # Triggers: EF Grippers
+            if left_trigger > 0 and right_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = 0.0
+            elif left_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = -1.0
+            elif right_trigger > 0:
+                arm_input.velocities[
+                    self.all_joint_names.index("ef_gripper_left_joint")
+                ] = 1.0
+
+            # Bumpers: EF roll
+            arm_input.velocities[self.all_joint_names.index("wrist_roll_joint")] = (
+                right_bumper - left_bumper
+            )
+
+            # A: brake
+            # TODO: Brake mode
+
+            # Y: linear actuator
+            # TODO: linear actuator
+
+            self.arm_manual_pub_.publish(arm_input)
+
+        # IK (ONLY NEW)
+        # =============
+
+        elif self.use_arm_ik:
+            arm_twist = TwistStamped()
+            arm_twist.header.frame_id = "base_link"
+            arm_twist.header.stamp = self.get_clock().now().to_msg()
+            arm_jointjog = JointJog()
+            arm_jointjog.header.frame_id = "base_link"
+            arm_jointjog.header.stamp = self.get_clock().now().to_msg()
+
+            # Right stick: linear y and linear x
+            # Left stick: angular z and linear z
+            # D-pad: angular y and _
+            # Triggers: EF grippers
+            # Bumpers: angular x
+            # A: brake
+            # B: IK mode
+            # X: manual mode
+            # Y: linear actuator
+
+            # Right stick: linear y and linear x
+            arm_twist.twist.linear.y = float(right_stick_x)
+            arm_twist.twist.linear.x = float(right_stick_y)
+
+            # Left stick: angular z and linear z
+            arm_twist.twist.angular.z = float(-1 * left_stick_x)
+            arm_twist.twist.linear.z = float(-1 * left_stick_y)
+            # D-pad: angular y and _
+            arm_twist.twist.angular.y = (
+                float(0)
+                if dpad_input[0] == 0
+                else float(-1 * copysign(0.75, dpad_input[0]))
+            )
+
+            # Triggers: EF Grippers
+            if left_trigger > 0 or right_trigger > 0:
+                arm_jointjog.joint_names.append("ef_gripper_left_joint")  # type: ignore
+                arm_jointjog.velocities.append(float(right_trigger - left_trigger))
+
+            # Bumpers: angular x
+            if left_bumper > 0 and right_bumper > 0:
+                arm_twist.twist.angular.x = float(0)
+            elif left_bumper > 0:
+                arm_twist.twist.angular.x = float(1)
+            elif right_bumper > 0:
+                arm_twist.twist.angular.x = float(-1)
+
+            self.arm_ik_twist_publisher.publish(arm_twist)
+            # self.arm_ik_jointjog_publisher.publish(arm_jointjog)  # TODO: Figure this shit out
+
+    def send_bio(self):
+        # Collect controller state
+        left_stick_x = stick_deadzone(self.gamepad.get_axis(0))
+        left_stick_y = stick_deadzone(self.gamepad.get_axis(1))
+        left_trigger = stick_deadzone(self.gamepad.get_axis(2))
+        right_stick_x = stick_deadzone(self.gamepad.get_axis(3))
+        right_stick_y = stick_deadzone(self.gamepad.get_axis(4))
+        right_trigger = stick_deadzone(self.gamepad.get_axis(5))
+        button_a = self.gamepad.get_button(0)
+        button_b = self.gamepad.get_button(1)
+        button_x = self.gamepad.get_button(2)
+        button_y = self.gamepad.get_button(3)
+        left_bumper = self.gamepad.get_button(4)
+        right_bumper = self.gamepad.get_button(5)
+        dpad_input = self.gamepad.get_hat(0)
+
+        if self.use_old_topics:
+            bio_input = BioControl(
+                bio_arm=int(left_stick_y * -100),
+                drill_arm=int(round(right_stick_y) * -100),
+            )
+
+            # Drill motor (FAERIE)
+            if left_trigger > 0 or right_trigger > 0:
+                bio_input.drill = int(
+                    30 * (right_trigger - left_trigger)
+                )  # Max duty cycle 30%
+
+            self.bio_publisher.publish(bio_input)
+
+        else:
+            pass  # TODO: implement new bio control topics
+
+    def arm_manual_stop_msg(self):
+        return JointJog(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg()),
+            joint_names=self.all_joint_names,
+            velocities=[0.0] * len(self.all_joint_names),
+        )
+
+    def arm_ik_twist_stop_msg(self):
+        return TwistStamped(
+            header=Header(frame_id="base_link", stamp=self.get_clock().now().to_msg())
+        )
 
 
-def deadzone(value: float, threshold=0.05) -> float:
-    """ Apply a deadzone to a joystick input so the motors don't sound angry """
+def stick_deadzone(value: float, threshold=STICK_DEADZONE) -> float:
+    """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 stick_to_arm_direction(value: float, threshold=ARM_DEADZONE) -> int:
+    """Apply a larger deadzone to a stick input and make digital/binary instead of analog"""
+    if abs(value) < threshold:
+        return 0
+    return int(copysign(1, value))
 
-if __name__ == '__main__':
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+
+def is_user_in_group(group_name: str) -> bool:
+    # Copied from https://zetcode.com/python/os-getgrouplist/
+    try:
+        username = os.getlogin()
+
+        # 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 exit_handler(signum, frame):
+    print("Caught SIGTERM. Exiting...")
+    rclpy.try_shutdown()
+    sys.exit(0)
+
+
+def main(args=None):
+    try:
+        rclpy.init(args=args)
+
+        # Catch termination signals and exit cleanly
+        signal.signal(signal.SIGTERM, exit_handler)
+
+        node = Headless()
+        rclpy.spin(node)
+    except (KeyboardInterrupt, ExternalShutdownException):
+        print("Caught shutdown signal. Exiting...")
+    finally:
+        rclpy.try_shutdown()
+
+
+if __name__ == "__main__":
     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/latency_tester/src/embedded_ping.cpp

@@ -26,7 +26,7 @@ class LatencyTester : public rclcpp::Node
 {
 public:
     LatencyTester()
-    : Node("latency_tester"), count_(0), target_mcu_("core")
+    : Node("latency_tester"), count_(0)
     {
         publisher_ = this->create_publisher<std_msgs::msg::String>("/anchor/relay", 10);
         timer_ = this->create_wall_timer(
@@ -35,6 +35,8 @@ public:
             "/anchor/debug",
             10,
             std::bind(&LatencyTester::response_callback, this, std::placeholders::_1));
+
+        target_mcu_ = this->declare_parameter<std::string>("target_mcu", "core");
     }
 
 private:

treefmt.nix

@@ -0,0 +1,8 @@
+{ pkgs, ... }:
+{
+  projectRootFile = "flake.nix";
+  programs = {
+    nixfmt.enable = true;
+    black.enable = true;
+  };
+}