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

switch to astra_msgs

.gitignore

@@ -14,3 +14,4 @@ __pycache__/
 
 #Direnv
 .direnv/
+.venv

.gitmodules

@@ -1,6 +1,6 @@
-[submodule "src/ros2_interfaces_pkg"]
-	path = src/ros2_interfaces_pkg
-	url = ../ros2_interfaces_pkg
 [submodule "src/astra_description"]
 	path = src/astra_descriptions
 	url = ../astra_descriptions
+[submodule "src/astra_msgs"]
+	path = src/astra_msgs
+	url = git@github.com:SHC-ASTRA/astra_msgs.git

README.md

@@ -1,89 +1,148 @@
-# rover-ros2
+# ASTRA Rover ROS2 Packages
 
 [![License: AGPL v3](https://img.shields.io/badge/License-AGPL_v3-blue.svg)](https://www.gnu.org/licenses/agpl-3.0)
 
-Submodule which includes all ros2 packages for the rover. These are centrally located for modular rover operation.
+Includes all main ROS2 packages for the rover. These are centrally located for modular rover operation.
 
-You will use this package to launch any module-side ROS2 nodes.
+You will use these packages to launch all rover-side ROS2 nodes.
-<br>
 
-## Software Pre-reqs
+## Table of Contents
 
-An acting base station computer will need several things:
+- [Software Prerequisites](#software-prerequisites)
+  - [Nix](#nix)
+  - [ROS2 Humble + rosdep](#ros2-humble--rosdep)
+- [Running](#running)
+  - [Testing Serial](#testing-serial)
+  - [Connecting the GuliKit Controller](#connecting-the-gulikit-controller)
+- [Common Problems/Toubleshooting](#common-problemstroubleshooting)
+- [Packages](#packages)
+- [Maintainers](#maintainers)
 
-* ROS2 Humble
+## Software Prerequisites
-    * 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
 
-## 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)
+### Nix
-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`
 
-## 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
+### ROS2 Humble + rosdep
-    * 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`
 
-## 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.
+```bash
-<br>
+  # Setup rosdep
-1. SSH to the the module's computer
+$ sudo rosdep init  # only run if you haven't already
-    * Core1: `ssh clucky@192.168.1.69`
+$ rosdep update
-    * Core2: `ssh clucky@192.168.1.70`
+  # Install dependencies
-    * Arm: `ssh arm@192.168.1.70`
+$ cd path/to/rover-ros2
-    * Password: \<can be found in the rover-Docs repo under documentation>
+$ rosdep install --from-paths src -y --ignore-src
-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`
 
-## Connecting the GuliKit Controller
+## Running
 
-Connecting the GuliKit Controller (Recommended)
+```bash
+$ colcon build
+$ source install/setup.bash
+  # main launch files:
+$ ros2 launch anchor_pkg rover.launch.py  # Must be run on a computer connected to a MCU on the rover.
+$ ros2 run headless_pkg headless_full  # Optionally run in a separate shell on the same or different computer.
+```
 
-* Connect controller to pc with USB-C
+### Testing Serial
+
+You can fake the presence of a Serial device (i.e., MCU) by using the following command:
+
+```bash
+$ socat -dd -v pty,rawer,crnl,link=/tmp/ttyACM9 pty,rawer,crnl,link=/tmp/ttyOUT
+```
+
+When you go to run anchor, use the `PORT_OVERRIDE` environment variable to point it to the fake serial port, like so:
+
+```bash
+$ PORT_OVERRIDE=/tmp/ttyACM9 ros2 launch anchor_pkg rover.launch.py
+```
+
+### Connecting the GuliKit Controller
+
+These instructions apply to the black XBox-style GuliKit controller, primarily used for controlling Arm through Basestation.
+
+* Connect the controller to your PC with a USB-C cable
 * Select the "X-Input" control mode (Windows logo) on the controller.
-* Hold the button next to the symbols (windows, android, switch, etc...)
+  * 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
+  * You'll need to release the button and press down again to cycle to the next mode
+
+## Common Problems/Troubleshooting
+
+**Q**: When I try to launch the nodes, I receive a `package '' not found` error.
+
+A: Make sure you have sourced the workspace in the current shell:
+
+```bash
+$ source install/setup.bash  # or setup.zsh if using ZSH
+```
+
+**Q**: When I try to launch the nodes, I receive several `FileNotFoundError: [Errno 2]` errors.
+
+A: Sometimes the install files get messed up by running `colcon build` in different shells or updating packages. Try running the following commands to clean up your local build files:
+
+```bash
+$ rm -rf build/ install/
+$ colcon build
+```
+
+**Q**: When I run `colcon build` after the above suggestion, I receive several of the following errors:
+
+```bash
+[0.557s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '' in the environment variable AMENT_PREFIX_PATH doesn't exist
+```
+
+A: Don't worry about it. If you had the workspace sourced, ROS2 will complain about the workspace install files not existing anymore after you deleted them. They will be re-created by `colcon build`, after which you can run `source install/setup.bash` to source the new install files.
+
+**Q**: When I try to launch Anchor, I receive the following errors:
+
+```bash
+[anchor-5] [INFO] [1762239452.937881841] [anchor]: Unable to find MCU...
+...
+[ERROR] [anchor-5]: process has died [pid 101820, exit code 1, cmd '.../rover-ros2/install/anchor_pkg/lib/anchor_pkg/anchor --ros-args -r __node:=anchor --params-file /tmp/launch_params_nmv6tpw4'].
+[INFO] [launch]: process[anchor-5] was required: shutting down launched system
+[INFO] [bio-4]: sending signal 'SIGINT' to process[bio-4]
+[INFO] [ptz-3]: sending signal 'SIGINT' to process[ptz-3]
+[INFO] [core-2]: sending signal 'SIGINT' to process[core-2]
+[INFO] [arm-1]: sending signal 'SIGINT' to process[arm-1]
+...
+```
+
+A: To find a microcontroller to talk to, Anchor sends a ping to every Serial port on your computer. If it does not receive a 'pong' in less than one second, then it aborts. There are a few possible fixes:
+
+- Keep trying to run it until it works
+- Run `lsusb` to see if the microcontroller is detected by your computer.
+- Run `ls /dev/tty*0` to see if there is a valid Serial port enumerated for the microcontroller.
+- Check if you are in the `dialout` group (or whatever group shows up by running `ls -l /dev/tty*`).
+
+## Packages
+
+- [anchor\_pkg](./src/anchor_pkg) - Handles Serial communication between the various other packages here and the microcontroller.
+- [arm\_pkg](./src/arm_pkg) - Relays controls and sensor data for the arm (socket and digit) between anchor and basestation/headless.
+- [astra\_descriptions](./src/astra_descriptions) - Submodule with URDF-related packages.
+- [bio\_pkg](./src/bio_pkg) - Like arm_pkg, but for CITADEL and FAERIE
+- [core\_pkg](./src/core_pkg) - Like arm_pkg, but for Core
+- [headless\_pkg](./src/headless_pkg) - Simple, non-graphical controller node to work in place of basestation when controlling the rover by itself. This is autostarted with anchor to allow for setup-less control of the rover.
+- [latency\_tester](./src/latency_tester) - A temporary node to test comms latency over ROS2, Serial, and CAN.
+- [ros2\_interfaces\_pkg](./src/ros2_interfaces_pkg) - Contains custom message types for communication between basestation and the rover over ROS2. (being renamed to `astra_msgs`).
+- [servo\_arm\_twist\_pkg](./src/servo_arm_twist_pkg) - A temporary node to translate controller state from `ros2_joy` to `Twist` messages to control the Arm via IK.
+
+## Maintainers
+
+| Name | Email | Discord |
+| ---- | ----- | ------- |
+| David Sharpe | <ds0196@uah.edu> | `@ddavdd` |
+| Riley McLain | <rjm0037@uah.edu> | `@ryleu` |

auto_start/auto_start_anchor.sh

@@ -1,4 +1,7 @@
-#!/bin/bash
+#!/usr/bin/env bash
+set -e
+
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
 source /opt/ros/humble/setup.bash
 
 # Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
+source $SCRIPT_DIR/../install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
 
 # Launch the ROS 2 node with the desired mode
 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
@@ -15,10 +18,7 @@ echo "[INFO] Starting ROS node..."
 source /opt/ros/humble/setup.bash
 
 # Source your workspace setup script
-source /home/clucky/rover-ros2/install/setup.bash
+source $SCRIPT_DIR/../install/setup.bash
-
-# CD to directory
-cd /home/clucky/rover-ros2/
 
 # 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"
+SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )
-AUTONOMY_WS="/home/clucky/rover-Autonomy"
+
-BAG_LOCATION="/home/clucky/bags/autostart"
+[[ -z "$ANCHOR_WS" ]] && ANCHOR_WS="$SCRIPT_DIR/.."
+[[ -z "$AUTONOMY_WS" ]] && AUTONOMY_WS="$HOME/rover-Autonomy"
+BAG_LOCATION="$HOME/bags/autostart"
+[[ ! -d "$BAG_LOCATION" ]] && mkdir -p "$BAG_LOCATION"
 
 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
@@ -15,7 +19,7 @@ echo "[INFO] Network interface is up!"
 
 source /opt/ros/humble/setup.bash
 source $ANCHOR_WS/install/setup.bash
-[ -f $AUTONOMY_WS/install/setup.bash ] && source $AUTONOMY_WS/install/setup.bash
+[[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
 
 cd $BAG_LOCATION
 

flake.lock

@@ -24,11 +24,11 @@
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1758094726,
+        "lastModified": 1761810010,
-        "narHash": "sha256-agLnClczRtYY+kQFh5dv4wGNhtFNKK7KFOmypDhsWCs=",
+        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "9d0557032aadb65df065b1972a632572b57234b5",
+        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
         "type": "github"
       },
       "original": {

flake.nix

@@ -18,6 +18,12 @@
         pkgs = import nixpkgs {
           inherit system;
           overlays = [ nix-ros-overlay.overlays.default ];
+          config = {
+            # Allow the insecure freeimage package
+            permittedInsecurePackages = [
+              "freeimage-3.18.0-unstable-2024-04-18"
+            ];
+          };
         };
       in
       {
@@ -35,7 +41,7 @@
               ]
             ))
             (
-              with rosPackages.humble;
+              with rosPackages.jazzy;
               buildEnv {
                 paths = [
                   ros-core
@@ -53,6 +59,24 @@
                   robot-state-publisher
                   ros2-control
                   controller-manager
+                  control-msgs
+                  control-toolbox
+                  moveit-core
+                  moveit-common
+                  moveit-msgs
+                  moveit-ros-planning
+                  moveit-ros-planning-interface
+                  moveit-configs-utils
+                  moveit-ros-move-group
+                  moveit-servo
+                  moveit-simple-controller-manager
+                  pilz-industrial-motion-planner
+                  pick-ik
+                  ompl
+                  chomp-motion-planner
+                  joy
+                  ros-gz-sim
+                  ros-gz-bridge
                   # ros2-controllers nixpkg does not build :(
                 ];
               }
@@ -62,6 +86,10 @@
             # Display stuff
             export DISPLAY=''${DISPLAY:-:0}
             export QT_X11_NO_MITSHM=1
+	    if [[ ! $DIRENV_IN_ENVRC ]]; then
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete ros2)"
+              eval "$(${pkgs.python3Packages.argcomplete}/bin/register-python-argcomplete colcon)"
+            fi
           '';
         };
       }

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -13,7 +13,7 @@ import threading
 import glob
 
 from std_msgs.msg import String, Header
-from ros2_interfaces_pkg.msg import VicCAN
+from astra_msgs.msg import VicCAN
 
 serial_pub = None
 thread = None
@@ -38,10 +38,12 @@ Subscribers:
   * /anchor/to_vic/relay_string
     - Publish raw strings to this topic to send directly to the MCU for debugging
 """
+
+
 class SerialRelay(Node):
     def __init__(self):
         # Initalize node with name
-        super().__init__("anchor_node")#previously 'serial_publisher'
+        super().__init__("anchor_node")  # previously 'serial_publisher'
 
         # Loop through all serial devices on the computer to check for the MCU
         self.port = None
@@ -55,7 +57,7 @@ class SerialRelay(Node):
                 try:
                     # connect and send a ping command
                     ser = serial.Serial(port, 115200, timeout=1)
-                    #(f"Checking port {port}...")
+                    # (f"Checking port {port}...")
                     ser.write(b"ping\n")
                     response = ser.read_until(bytes("\n", "utf8"))
 
@@ -78,26 +80,40 @@ class SerialRelay(Node):
         atexit.register(self.cleanup)
 
         # New pub/sub with VicCAN
-        self.fromvic_debug_pub_ = self.create_publisher(String, '/anchor/from_vic/debug', 20)
+        self.fromvic_debug_pub_ = self.create_publisher(
-        self.fromvic_core_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/core', 20)
+            String, "/anchor/from_vic/debug", 20
-        self.fromvic_arm_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/arm', 20)
+        )
-        self.fromvic_bio_pub_ = self.create_publisher(VicCAN, '/anchor/from_vic/bio', 20)
+        self.fromvic_core_pub_ = self.create_publisher(
-
+            VicCAN, "/anchor/from_vic/core", 20
-        self.mock_mcu_sub_ = self.create_subscription(String, '/anchor/from_vic/mock_mcu', self.on_mock_fromvic, 20)
+        )
-        self.tovic_sub_ = self.create_subscription(VicCAN, '/anchor/to_vic/relay', self.on_relay_tovic_viccan, 20)
+        self.fromvic_arm_pub_ = self.create_publisher(
-        self.tovic_debug_sub_ = self.create_subscription(String, '/anchor/to_vic/relay_string', self.on_relay_tovic_string, 20)
+            VicCAN, "/anchor/from_vic/arm", 20
+        )
+        self.fromvic_bio_pub_ = self.create_publisher(
+            VicCAN, "/anchor/from_vic/bio", 20
+        )
 
+        self.mock_mcu_sub_ = self.create_subscription(
+            String, "/anchor/from_vic/mock_mcu", self.on_mock_fromvic, 20
+        )
+        self.tovic_sub_ = self.create_subscription(
+            VicCAN, "/anchor/to_vic/relay", self.on_relay_tovic_viccan, 20
+        )
+        self.tovic_debug_sub_ = self.create_subscription(
+            String, "/anchor/to_vic/relay_string", self.on_relay_tovic_string, 20
+        )
 
         # Create publishers
-        self.arm_pub = self.create_publisher(String, '/anchor/arm/feedback', 10)
+        self.arm_pub = self.create_publisher(String, "/anchor/arm/feedback", 10)
-        self.core_pub = self.create_publisher(String, '/anchor/core/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.bio_pub = self.create_publisher(String, "/anchor/bio/feedback", 10)
 
-        self.debug_pub = self.create_publisher(String, '/anchor/debug', 10)
+        self.debug_pub = self.create_publisher(String, "/anchor/debug", 10)
 
         # Create a subscriber
-        self.relay_sub = self.create_subscription(String, '/anchor/relay', self.on_relay_tovic_string, 10)
+        self.relay_sub = self.create_subscription(
-
+            String, "/anchor/relay", self.on_relay_tovic_string, 10
+        )
 
     def run(self):
         # This thread makes all the update processes run in the background
@@ -112,22 +128,26 @@ class SerialRelay(Node):
             sys.exit(0)
 
     def read_MCU(self):
-        """ Check the USB serial port for new data from the MCU, and publish string to appropriate topics """
+        """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
         try:
             output = str(self.ser.readline(), "utf8")
 
             if output:
                 self.relay_fromvic(output)
                 # All output over debug temporarily
-                #self.get_logger().info(f"[MCU] {output}")
+                # self.get_logger().info(f"[MCU] {output}")
                 msg = String()
                 msg.data = output
                 self.debug_pub.publish(msg)
                 if output.startswith("can_relay_fromvic,core"):
                     self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"):  # digit for voltage readings
+                elif output.startswith("can_relay_fromvic,arm") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for voltage readings
                     self.arm_pub.publish(msg)
-                if output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"):  # digit for SHT sensor
+                if output.startswith("can_relay_fromvic,citadel") or output.startswith(
+                    "can_relay_fromvic,digit"
+                ):  # digit for SHT sensor
                     self.bio_pub.publish(msg)
                 # msg = String()
                 # msg.data = output
@@ -152,15 +172,13 @@ class SerialRelay(Node):
             #     self.ser.close()
             # exit(1)
 
-
     def on_mock_fromvic(self, msg: String):
-        """ For testing without an actual MCU, publish strings here as if they came from an MCU """
+        """For testing without an actual MCU, publish strings here as if they came from an MCU"""
         # self.get_logger().info(f"Got command from mock MCU: {msg}")
         self.relay_fromvic(msg.data)
 
-
     def on_relay_tovic_viccan(self, msg: VicCAN):
-        """ Relay a VicCAN message to the MCU """
+        """Relay a VicCAN message to the MCU"""
         output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
         for num in msg.data:
             output += f",{round(num, 7)}"  # limit to 7 decimal places
@@ -169,7 +187,7 @@ class SerialRelay(Node):
         self.ser.write(bytes(output, "utf8"))
 
     def relay_fromvic(self, msg: str):
-        """ Relay a string message from the MCU to the appropriate VicCAN topic """
+        """Relay a string message from the MCU to the appropriate VicCAN topic"""
         self.fromvic_debug_pub_.publish(String(data=msg))
         parts = msg.strip().split(",")
         if len(parts) > 0 and parts[0] != "can_relay_fromvic":
@@ -181,11 +199,13 @@ class SerialRelay(Node):
         malformed_reason: str = ""
         if len(parts) < 3 or len(parts) > 7:
             malformed = True
-            malformed_reason = f"invalid argument count (expected [3,7], got {len(parts)})"
+            malformed_reason = (
+                f"invalid argument count (expected [3,7], got {len(parts)})"
+            )
         elif parts[1] not in ["core", "arm", "digit", "citadel", "broadcast"]:
             malformed = True
             malformed_reason = f"invalid mcu_name '{parts[1]}'"
-        elif not(parts[2].isnumeric()) or int(parts[2]) < 0:
+        elif not (parts[2].isnumeric()) or int(parts[2]) < 0:
             malformed = True
             malformed_reason = f"command_id '{parts[2]}' is not a non-negative integer"
         else:
@@ -198,7 +218,9 @@ class SerialRelay(Node):
                     break
 
         if malformed:
-            self.get_logger().warning(f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}")
+            self.get_logger().warning(
+                f"Ignoring malformed from_vic message: '{msg.strip()}'; reason: {malformed_reason}"
+            )
             return
 
         # Have valid VicCAN message
@@ -208,7 +230,9 @@ class SerialRelay(Node):
         output.command_id = int(parts[2])
         if len(parts) > 3:
             output.data = [float(x) for x in parts[3:]]
-        output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic")
+        output.header = Header(
+            stamp=self.get_clock().now().to_msg(), frame_id="from_vic"
+        )
 
         # self.get_logger().info(f"Relaying from MCU: {output}")
         if output.mcu_name == "core":
@@ -218,11 +242,10 @@ class SerialRelay(Node):
         elif output.mcu_name == "citadel" or output.mcu_name == "digit":
             self.fromvic_bio_pub_.publish(output)
 
-
     def on_relay_tovic_string(self, msg: String):
-        """ Relay a raw string message to the MCU for debugging """
+        """Relay a raw string message to the MCU for debugging"""
         message = msg.data
-        #self.get_logger().info(f"Sending command to MCU: {msg}")
+        # self.get_logger().info(f"Sending command to MCU: {msg}")
         self.ser.write(bytes(message, "utf8"))
 
     @staticmethod
@@ -234,6 +257,7 @@ class SerialRelay(Node):
         if self.ser.is_open:
             self.ser.close()
 
+
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
     if serial_pub:
@@ -249,7 +273,10 @@ def main(args=None):
     serial_pub = SerialRelay()
     serial_pub.run()
 
-if __name__ == '__main__':
+
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+if __name__ == "__main__":
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    # 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/anchor_pkg/launch/rover.launch.py

@@ -2,115 +2,121 @@
 
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
-from launch.substitutions import LaunchConfiguration
+from launch.substitutions import (
+    LaunchConfiguration,
+    ThisLaunchFileDir,
+    PathJoinSubstitution,
+)
 from launch_ros.actions import Node
 
 
-
+# Prevent making __pycache__ directories
-#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)
+    mode = LaunchConfiguration("mode").perform(context)
     nodes = []
 
-    if mode == 'anchor':
+    if mode == "anchor":
         # Launch every node and pass "anchor" as the parameter
+
         nodes.append(
             Node(
-                package='arm_pkg',
+                package="arm_pkg",
-                executable='arm',  # change as needed
+                executable="arm",  # change as needed
-                name='arm',
+                name="arm",
-                output='both',
+                output="both",
-                parameters=[{'launch_mode': mode}],
+                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown()
+                on_exit=Shutdown(),
             )
         )
         nodes.append(
             Node(
-                package='core_pkg',
+                package="core_pkg",
-                executable='core',  # change as needed
+                executable="core",  # change as needed
-                name='core',
+                name="core",
-                output='both',
+                output="both",
-                parameters=[{'launch_mode': mode}],
+                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown()
+                on_exit=Shutdown(),
             )
         )
         nodes.append(
             Node(
-                package='core_pkg',
+                package="core_pkg",
-                executable='ptz',  # change as needed
+                executable="ptz",  # change as needed
-                name='ptz',
+                name="ptz",
-                output='both'
+                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',
+                package="bio_pkg",
-                executable='bio',  # change as needed
+                executable="bio",  # change as needed
-                name='bio',
+                name="bio",
-                output='both',
+                output="both",
-                parameters=[{'launch_mode': mode}],
+                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown()
+                on_exit=Shutdown(),
             )
         )
         nodes.append(
             Node(
-                package='anchor_pkg',
+                package="anchor_pkg",
-                executable='anchor',  # change as needed
+                executable="anchor",  # change as needed
-                name='anchor',
+                name="anchor",
-                output='both',
+                output="both",
-                parameters=[{'launch_mode': mode}],
+                parameters=[{"launch_mode": mode}],
-                on_exit=Shutdown()
+                on_exit=Shutdown(),
             )
         )
-    elif mode in ['arm', 'core', 'bio', 'ptz']:
+    elif mode in ["arm", "core", "bio", "ptz"]:
         # Only launch the node corresponding to the provided mode.
-        if mode == 'arm':
+        if mode == "arm":
             nodes.append(
                 Node(
-                    package='arm_pkg',
+                    package="arm_pkg",
-                    executable='arm',
+                    executable="arm",
-                    name='arm',
+                    name="arm",
-                    output='both',
+                    output="both",
-                    parameters=[{'launch_mode': mode}],
+                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown()
+                    on_exit=Shutdown(),
                 )
             )
-        elif mode == 'core':
+        elif mode == "core":
             nodes.append(
                 Node(
-                    package='core_pkg',
+                    package="core_pkg",
-                    executable='core',
+                    executable="core",
-                    name='core',
+                    name="core",
-                    output='both',
+                    output="both",
-                    parameters=[{'launch_mode': mode}],
+                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown()
+                    on_exit=Shutdown(),
                 )
             )
-        elif mode == 'bio':
+        elif mode == "bio":
             nodes.append(
                 Node(
-                    package='bio_pkg',
+                    package="bio_pkg",
-                    executable='bio',
+                    executable="bio",
-                    name='bio',
+                    name="bio",
-                    output='both',
+                    output="both",
-                    parameters=[{'launch_mode': mode}],
+                    parameters=[{"launch_mode": mode}],
-                    on_exit=Shutdown()
+                    on_exit=Shutdown(),
                 )
             )
-        elif mode == 'ptz':
+        elif mode == "ptz":
             nodes.append(
                 Node(
-                    package='core_pkg',
+                    package="core_pkg",
-                    executable='ptz',
+                    executable="ptz",
-                    name='ptz',
+                    name="ptz",
-                    output='both',
+                    output="both",
-                    on_exit=Shutdown(), #on fail, shutdown if this was the only node to be launched
+                    on_exit=Shutdown(),  # on fail, shutdown if this was the only node to be launched
                 )
             )
     else:
@@ -119,14 +125,12 @@ def launch_setup(context, *args, **kwargs):
 
     return nodes
 
+
 def generate_launch_description():
     declare_arg = DeclareLaunchArgument(
-        'mode',
+        "mode",
-        default_value='anchor',
+        default_value="anchor",
-        description='Launch mode: arm, core, bio, anchor, or ptz'
+        description="Launch mode: arm, core, bio, anchor, or ptz",
     )
 
-    return LaunchDescription([
+    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
-        declare_arg,
-        OpaqueFunction(function=launch_setup)
-    ])

src/anchor_pkg/setup.py

@@ -2,27 +2,24 @@ 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',
+    version="0.0.0",
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-            ['resource/' + package_name]),
+        (path.join("share", package_name), ["package.xml"]),
-        (path.join("share", package_name), ['package.xml']),
+        (path.join("share", package_name, "launch"), glob("launch/*")),
-        (path.join("share", package_name, "launch"), glob("launch/*"))
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='tristan',
+    maintainer="tristan",
-    maintainer_email='tristanmcginnis26@gmail.com',
+    maintainer_email="tristanmcginnis26@gmail.com",
-    description='Anchor node used to run all modules through a single modules MCU/Computer. Commands to all modules will be relayed through CAN',
+    description="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',
+    license="All Rights Reserved",
     entry_points={
-        'console_scripts': [
+        "console_scripts": ["anchor = anchor_pkg.anchor_node:main"],
-            "anchor = anchor_pkg.anchor_node:main"
-        ],
     },
 )

src/arm_pkg/arm_pkg/arm_headless.py

@@ -12,14 +12,17 @@ import glob
 import os
 
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import ControllerState
+from astra_msgs.msg import ControllerState
-from ros2_interfaces_pkg.msg import ArmManual
+from astra_msgs.msg import ArmManual
-from ros2_interfaces_pkg.msg import ArmIK
+from astra_msgs.msg import ArmIK
 
 
-os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+os.environ["SDL_AUDIODRIVER"] = (
+    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+)
 os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
 
+
 class Headless(Node):
     def __init__(self):
         # Initalize node with name
@@ -30,20 +33,18 @@ class Headless(Node):
 
         self.create_timer(0.1, self.send_manual)
 
-
         # Create a publisher to publish any output the pico sends
 
         # Depricated, kept temporarily for reference
-        #self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10) 
+        # self.publisher = self.create_publisher(ControllerState, '/astra/arm/control', 10)
-        self.manual_pub = self.create_publisher(ArmManual, '/arm/control/manual', 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.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.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
 
         self.laser_status = 0
 
@@ -66,38 +67,36 @@ class Headless(Node):
         # Initialize the first gamepad, print name to terminal
         self.gamepad = pygame.joystick.Joystick(0)
         self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
+        print(f"Gamepad Found: {self.gamepad.get_name()}")
         #
         #
 
-
     def run(self):
         # This thread makes all the update processes run in the background
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
         thread.start()
 
-
         try:
             while rclpy.ok():
-                #Check the pico for updates
+                # Check the pico for updates
 
-                #self.read_feedback()
+                # self.read_feedback()
-                if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
+                if (
+                    pygame.joystick.get_count() == 0
+                ):  # if controller disconnected, wait for it to be reconnected
                     print(f"Gamepad disconnected: {self.gamepad.get_name()}")
 
                     while pygame.joystick.get_count() == 0:
-                        #self.send_controls() #depricated, kept for reference temporarily
+                        # self.send_controls() #depricated, kept for reference temporarily
                         self.send_manual()
-                        #self.read_feedback()
+                        # self.read_feedback()
                     self.gamepad = pygame.joystick.Joystick(0)
-                    self.gamepad.init() #re-initialized gamepad
+                    self.gamepad.init()  # re-initialized gamepad
                     print(f"Gamepad reconnected: {self.gamepad.get_name()}")
 
-
         except KeyboardInterrupt:
             sys.exit(0)
 
-
     def send_manual(self):
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
@@ -105,21 +104,20 @@ class Headless(Node):
                 exit()
         input = ArmManual()
 
-
         # Triggers for gripper control
-        if self.gamepad.get_axis(2) > 0:#left trigger
+        if self.gamepad.get_axis(2) > 0:  # left trigger
             input.gripper = -1
-        elif self.gamepad.get_axis(5) > 0:#right trigger
+        elif self.gamepad.get_axis(5) > 0:  # right trigger
             input.gripper = 1
 
         # Toggle Laser
-        if self.gamepad.get_button(7):#Start
+        if self.gamepad.get_button(7):  # Start
             self.laser_status = 1
-        elif self.gamepad.get_button(6):#Back
+        elif self.gamepad.get_button(6):  # Back
             self.laser_status = 0
         input.laser = self.laser_status
 
-        if self.gamepad.get_button(5):#right bumper, control effector
+        if self.gamepad.get_button(5):  # right bumper, control effector
 
             # Left stick X-axis for effector yaw
             if self.gamepad.get_axis(0) > 0:
@@ -141,38 +139,38 @@ class Headless(Node):
             elif dpad_input[0] == -1:
                 input.axis0 = -1
 
-            if self.gamepad.get_axis(0) > .15 or self.gamepad.get_axis(0) < -.15:
+            if self.gamepad.get_axis(0) > 0.15 or self.gamepad.get_axis(0) < -0.15:
                 input.axis1 = round(self.gamepad.get_axis(0))
 
-            if self.gamepad.get_axis(1) > .15 or self.gamepad.get_axis(1) < -.15:
+            if self.gamepad.get_axis(1) > 0.15 or self.gamepad.get_axis(1) < -0.15:
                 input.axis2 = -1 * round(self.gamepad.get_axis(1))
 
-            if self.gamepad.get_axis(4) > .15 or self.gamepad.get_axis(4) < -.15:
+            if self.gamepad.get_axis(4) > 0.15 or self.gamepad.get_axis(4) < -0.15:
                 input.axis3 = -1 * round(self.gamepad.get_axis(4))
 
             # input.axis1 = -1 * round(self.gamepad.get_axis(0))#left x-axis
             # input.axis2 = -1 * round(self.gamepad.get_axis(1))#left y-axis
             # input.axis3 = -1 * round(self.gamepad.get_axis(4))#right y-axis
 
-
+        # Button Mappings
-        #Button Mappings
+        # axis2 -> LT
-        #axis2 -> LT
+        # axis5 -> RT
-        #axis5 -> RT
+        # Buttons0 -> A
-        #Buttons0 -> A
+        # Buttons1 -> B
-        #Buttons1 -> B
+        # Buttons2 -> X
-        #Buttons2 -> X
+        # Buttons3 -> Y
-        #Buttons3 -> Y
+        # Buttons4 -> LB
-        #Buttons4 -> LB
+        # Buttons5 -> RB
-        #Buttons5 -> RB
+        # Buttons6 -> Back
-        #Buttons6 -> Back
+        # Buttons7 -> Start
-        #Buttons7 -> Start
 
         input.linear_actuator = 0
 
-
         if pygame.joystick.get_count() != 0:
 
-            self.get_logger().info(f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n")
+            self.get_logger().info(
+                f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
+            )
             self.manual_pub.publish(input)
         else:
             pass
@@ -241,14 +239,12 @@ class Headless(Node):
     #     else:
     #         input.y = False
 
-
     #     dpad_input = self.gamepad.get_hat(0)#D-pad input
 
     #     #not using up/down on DPad
     #     input.d_up = False
     #     input.d_down = False
 
-
     #     if(dpad_input[0] == 1):#D-pad right
     #         input.d_right = True
     #     else:
@@ -258,7 +254,6 @@ class Headless(Node):
     #     else:
     #         input.d_left = False
 
-
     #     if pygame.joystick.get_count() != 0:
 
     #         self.get_logger().info(f"[Ctrl] Updated Controller State\n")
@@ -268,9 +263,6 @@ class Headless(Node):
     #         pass
 
 
-
-        
-
 def main(args=None):
     rclpy.init(args=args)
 
@@ -279,9 +271,9 @@ def main(args=None):
     rclpy.spin(node)
     rclpy.shutdown()
 
-    #tb_bs = BaseStation()
+    # tb_bs = BaseStation()
-    #node.run()
+    # node.run()
 
 
-if __name__ == '__main__':
+if __name__ == "__main__":
     main()

src/arm_pkg/arm_pkg/arm_node.py

@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy import qos
 import serial
 import sys
 import threading
@@ -9,23 +8,11 @@ import time
 import atexit
 import signal
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import ArmManual
+from astra_msgs.msg import ArmManual
-from ros2_interfaces_pkg.msg import ArmIK
+from astra_msgs.msg import SocketFeedback
-from ros2_interfaces_pkg.msg import SocketFeedback
+from astra_msgs.msg import DigitFeedback
-from ros2_interfaces_pkg.msg import DigitFeedback
+from sensor_msgs.msg import JointState
-
+import math
-
-# IK-Related imports
-import numpy as np
-import time, math, os
-from math import sin, cos, pi
-from ament_index_python.packages import get_package_share_directory
-# from ikpy.chain import Chain
-# from ikpy.link import OriginLink, URDFLink
-# #import pygame as pyg
-# from scipy.spatial.transform import Rotation as R
-
-from . import astra_arm
 
 # control_qos = qos.QoSProfile(
 #     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -48,42 +35,64 @@ class SerialRelay(Node):
         super().__init__("arm_node")
 
         # Get launch mode parameter
-        self.declare_parameter('launch_mode', 'arm')
+        self.declare_parameter("launch_mode", "arm")
-        self.launch_mode = self.get_parameter('launch_mode').value
+        self.launch_mode = self.get_parameter("launch_mode").value
         self.get_logger().info(f"arm launch_mode is: {self.launch_mode}")
 
         # Create publishers
-        self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10)
+        self.debug_pub = self.create_publisher(String, "/arm/feedback/debug", 10)
-        self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10)
+        self.socket_pub = self.create_publisher(
-        self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10)
+            SocketFeedback, "/arm/feedback/socket", 10
+        )
+        self.digit_pub = self.create_publisher(DigitFeedback, "/arm/feedback/digit", 10)
         self.feedback_timer = self.create_timer(0.25, self.publish_feedback)
 
         # Create subscribers
-        self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10)
+        self.man_sub = self.create_subscription(
-        self.man_sub = self.create_subscription(ArmManual, '/arm/control/manual', self.send_manual, 10)
+            ArmManual, "/arm/control/manual", self.send_manual, 10
+        )
 
-        self.ik_debug = self.create_publisher(String, '/arm/debug/ik', 10)
+        # New messages
+        self.joint_state_pub = self.create_publisher(JointState, "joint_states", 10)
+        self.joint_state = JointState()
+        self.joint_state.name = [
+            "Axis_0_Joint",
+            "Axis_1_Joint",
+            "Axis_2_Joint",
+            "Axis_3_Joint",
+            "Wrist_Differential_Joint",
+            "Wrist-EF_Roll_Joint",
+            "Gripper_Slider_Left",
+        ]
+        self.joint_state.position = [0.0] * len(
+            self.joint_state.name
+        )  # Initialize with zeros
+
+        self.joint_command_sub = self.create_subscription(
+            JointState, "/joint_commands", self.joint_command_callback, 10
+        )
 
         # Topics used in anchor mode
-        if self.launch_mode == 'anchor':
+        if self.launch_mode == "anchor":
-            self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10)
+            self.anchor_sub = self.create_subscription(
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+                String, "/anchor/arm/feedback", self.anchor_feedback, 10
+            )
+            self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        self.arm = astra_arm.Arm('arm12.urdf')
         self.arm_feedback = SocketFeedback()
         self.digit_feedback = DigitFeedback()
 
         # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        if self.launch_mode == 'arm':
+        if self.launch_mode == "arm":
             # Loop through all serial devices on the computer to check for the MCU
             self.port = None
             ports = SerialRelay.list_serial_ports()
-            for i in range(4):
+            for _ in range(4):
                 for port in ports:
                     try:
                         # connect and send a ping command
                         ser = serial.Serial(port, 115200, timeout=1)
-                        #print(f"Checking port {port}...")
+                        # print(f"Checking port {port}...")
                         ser.write(b"ping\n")
                         response = ser.read_until("\n")  # type: ignore
 
@@ -98,7 +107,9 @@ class SerialRelay(Node):
                     break
 
             if self.port is None:
-                self.get_logger().info("Unable to find MCU... please make sure it is connected.")
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
+                )
                 time.sleep(1)
                 sys.exit(1)
 
@@ -110,11 +121,11 @@ class SerialRelay(Node):
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
 
-        #if in arm mode, will need to read from the MCU 
+        # if in arm mode, will need to read from the MCU
 
         try:
             while rclpy.ok():
-                if self.launch_mode == 'arm':
+                if self.launch_mode == "arm":
                     if self.ser.in_waiting:
                         self.read_mcu()
                     else:
@@ -124,13 +135,12 @@ class SerialRelay(Node):
         finally:
             self.cleanup()
 
-
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
-    #Currently will just spit out all values over the /arm/feedback/debug topic as strings
     def read_mcu(self):
         try:
             output = str(self.ser.readline(), "utf8")
             if output:
-                #self.get_logger().info(f"[MCU] {output}")
+                # self.get_logger().info(f"[MCU] {output}")
                 msg = String()
                 msg.data = output
                 self.debug_pub.publish(msg)
@@ -152,79 +162,19 @@ class SerialRelay(Node):
                 self.ser.close()
             pass
 
-    def send_ik(self, msg):
+    def joint_command_callback(self, msg: JointState):
-        # Convert Vector3 to a NumPy array
+        # Embedded takes deg*10, ROS2 uses Radians
-        input_raw = np.array([-msg.movement_vector.x, msg.movement_vector.y, msg.movement_vector.z])  # Convert input to a NumPy array
+        positions = [math.degrees(pos) * 10 for pos in msg.position]
-        # decrease input vector by 90%
+        # Axis 2 & 3 URDF direction is inverted
-        input_raw = input_raw * 0.2
+        positions[2] = -positions[2]
+        positions[3] = -positions[3]
 
-        if input_raw[0] == 0.0 and input_raw[1] == 0.0 and input_raw[2] == 0.0:
+        # Set target angles for each arm axis for embedded IK PID to handle
-            self.get_logger().info("No input, stopping arm.")
+        command = f"can_relay_tovic,arm,32,{positions[0]},{positions[1]},{positions[2]},{positions[3]}\n"
-            command = "can_relay_tovic,arm,39,0,0,0,0\n"
+        # Wrist yaw and roll
+        command += f"can_relay_tovic,digit,32,{positions[4]},{positions[5]}\n"
+        # Gripper IK does not have adequate hardware yet
         self.send_cmd(command)
-            return
-
-        # Debug output
-        tempMsg = String()
-        tempMsg.data = "From IK Control Got Vector: " + str(input_raw)
-        #self.debug_pub.publish(tempMsg)
-
-        # Target position is current position + input vector
-        current_position = self.arm.get_position_vector()
-        target_position = current_position + input_raw
-
-
-        #Print for IK DEBUG
-        tempMsg = String()
-        # tempMsg.data = "Current Position: " + str(current_position) + "\nInput Vector" + str(input_raw) + "\nTarget Position: " + str(target_position) + "\nAngles: " + str(self.arm.current_angles)
-        tempMsg.data = "Current Angles: " + str(math.degrees(self.arm.current_angles[2])) + ", " + str(math.degrees(self.arm.current_angles[4])) + ", " + str(math.degrees(self.arm.current_angles[6]))
-        self.ik_debug.publish(tempMsg)
-        self.get_logger().info(f"[IK] {tempMsg.data}")
-
-        # Debug output for current position
-        #tempMsg.data = "Current Position: " + str(current_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Debug output for target position
-        #tempMsg.data = "Target Position: " + str(target_position)
-        #self.debug_pub.publish(tempMsg)
-
-        # Perform IK with the target position
-        if self.arm.perform_ik(target_position, self.get_logger()):
-            # Send command to control
-            
-            #command = "can_relay_tovic,arm,32," + ",".join(map(str, self.arm.ik_angles[:4])) + "\n"
-            #self.send_cmd(command)
-            self.get_logger().info(f"IK Success: {target_position}")
-            self.get_logger().info(f"IK Angles: [{str(round(math.degrees(self.arm.ik_angles[2]), 2))}, {str(round(math.degrees(self.arm.ik_angles[4]), 2))}, {str(round(math.degrees(self.arm.ik_angles[6]), 2))}]")
-
-            # tempMsg = String()
-            # tempMsg.data = "IK Success: " + str(target_position)
-            # #self.debug_pub.publish(tempMsg)
-            # tempMsg.data = "Sending: " + str(command)
-            #self.debug_pub.publish(tempMsg)
-
-            # Send the IK angles to the MCU
-            command = "can_relay_tovic,arm,32," + f"{math.degrees(self.arm.ik_angles[0])*10},{math.degrees(self.arm.ik_angles[2])*10},{math.degrees(self.arm.ik_angles[4])*10},{math.degrees(self.arm.ik_angles[6])*10}" + "\n"
-            # self.send_cmd(command)
-
-            # Manual control for Wrist/Effector
-            command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
-            
-            command += "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"
-
-            command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
-
-            command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
-
-            self.send_cmd(command)
-        else:
-            self.get_logger().info("IK Fail")
-            self.get_logger().info(f"IK Angles: [{str(math.degrees(self.arm.ik_angles[2]))}, {str(math.degrees(self.arm.ik_angles[4]))}, {str(math.degrees(self.arm.ik_angles[6]))}]")
-            # tempMsg = String()
-            # tempMsg.data = "IK Fail"
-            #self.debug_pub.publish(tempMsg)
-
 
     def send_manual(self, msg: ArmManual):
         axis0 = msg.axis0
@@ -232,42 +182,42 @@ class SerialRelay(Node):
         axis2 = msg.axis2
         axis3 = msg.axis3
 
-        #Send controls for arm
+        # Send controls for arm
-        command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
+        command = f"can_relay_tovic,arm,18,{int(msg.brake)}\n"
-        command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
+        command += f"can_relay_tovic,arm,39,{axis0},{axis1},{axis2},{axis3}\n"
 
-        #Send controls for end effector
+        # Send controls for end effector
 
-        # command += "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
+        command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{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"
 
-        command += "can_relay_tovic,digit,26," + str(msg.gripper) + "\n"
+        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
-        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 
-        command += "can_relay_tovic,digit,34," + str(msg.linear_actuator) + "\n"
+        command += f"can_relay_tovic,digit,34,{msg.linear_actuator}\n"
 
         self.send_cmd(command)
 
         return
 
     def send_cmd(self, msg: str):
-        if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay
+        if (
+            self.launch_mode == "anchor"
+        ):  # if in anchor mode, send to anchor node to relay
             output = String()
             output.data = msg
             self.anchor_pub.publish(output)
-        elif self.launch_mode == 'arm': #if in standalone mode, send to MCU directly
+        elif self.launch_mode == "arm":  # if in standalone mode, send to MCU directly
             self.get_logger().info(f"[Arm to MCU] {msg}")
             self.ser.write(bytes(msg, "utf8"))
 
     def anchor_feedback(self, msg: String):
         output = msg.data
         if output.startswith("can_relay_fromvic,arm,55"):
-            #pass
+            # pass
             self.updateAngleFeedback(output)
         elif output.startswith("can_relay_fromvic,arm,54"):
-            #pass
+            # pass
             self.updateBusVoltage(output)
         elif output.startswith("can_relay_fromvic,arm,53"):
             self.updateMotorFeedback(output)
@@ -285,6 +235,12 @@ class SerialRelay(Node):
             if len(parts) >= 4:
                 self.digit_feedback.wrist_angle = float(parts[3])
                 # self.digit_feedback.wrist_roll = float(parts[4])
+                self.joint_state.position[4] = math.radians(
+                    float(parts[4])
+                )  # Wrist roll
+                self.joint_state.position[5] = math.radians(
+                    float(parts[3])
+                )  # Wrist yaw
         else:
             return
 
@@ -302,28 +258,21 @@ class SerialRelay(Node):
             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
+            # Joint state publisher for URDF visualization
-            # tempMsg = String()
+            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
-            # tempMsg.data = "Angles: " + str(angles)
+            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
-            # #self.debug_pub.publish(tempMsg)
+            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
+            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
+            # Wrist is handled by digit feedback
+            self.joint_state.header.stamp = self.get_clock().now().to_msg()
+            self.joint_state_pub.publish(self.joint_state)
+
         else:
             self.get_logger().info("Invalid angle feedback input format")
 
@@ -364,11 +313,10 @@ class SerialRelay(Node):
             self.arm_feedback.axis0_voltage = voltage
             self.arm_feedback.axis0_current = current
 
-
     @staticmethod
     def list_serial_ports():
         return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        #return glob.glob("/dev/tty[A-Za-z]*")
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
     def cleanup(self):
         print("Cleaning up...")
@@ -378,11 +326,13 @@ class SerialRelay(Node):
         except Exception as e:
             exit(0)
 
+
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
     if serial_pub:
         serial_pub.cleanup()
 
+
 def main(args=None):
     rclpy.init(args=args)
     sys.excepthook = myexcepthook
@@ -391,7 +341,10 @@ def main(args=None):
     serial_pub = SerialRelay()
     serial_pub.run()
 
-if __name__ == '__main__':
+
-    #signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+if __name__ == "__main__":
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

src/arm_pkg/arm_pkg/astra_arm.py

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

src/arm_pkg/package.xml

@@ -10,9 +10,7 @@
   <depend>rclpy</depend>
   <depend>common_interfaces</depend>
   <depend>python3-numpy</depend>
-  <depend>ros2_interfaces_pkg</depend>
+  <depend>astra_msgs</depend>
-  <!-- TODO: remove after refactored out -->
-  <exec_depend>python3-ikpy-pip</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>

src/arm_pkg/setup.cfg

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

src/arm_pkg/setup.py

@@ -2,29 +2,26 @@ from setuptools import find_packages, setup
 import os
 from glob import glob
 
-package_name = 'arm_pkg'
+package_name = "arm_pkg"
 
 setup(
     name=package_name,
-    version='0.0.0',
+    version="1.0.0",
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-            ['resource/' + package_name]),
+        ("share/" + package_name, ["package.xml"]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*')),
-  	    (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='tristan',
+    maintainer="tristan",
-    maintainer_email='tristanmcginnis26@gmail.com',
+    maintainer_email="tristanmcginnis26@gmail.com",
-    description='TODO: Package description',
+    description="TODO: Package description",
-    license='All Rights Reserved',
+    license="All Rights Reserved",
     entry_points={
-        'console_scripts': [
+        "console_scripts": [
-            'arm = arm_pkg.arm_node:main',
+            "arm = arm_pkg.arm_node:main",
-            'headless = arm_pkg.arm_headless:main'
+            "headless = arm_pkg.arm_headless:main",
         ],
     },
 )

src/arm_pkg/urdf/arm11.urdf

@@ -1,239 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!--MX64trntbl + MX106:  ? + 0.153 -->
-  <!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
-  <!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
-  <material name="omni/Blue">
-    <color rgba="0 0 0.8 1"/>
-  </material>
-  <material name="omni/Red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="omni/Green">
-    <color rgba="0 1 0 1"/>
-  </material>
-  <material name="omni/Yellow">
-    <color rgba="1 1 0 1"/>
-  </material>
-  <material name="omni/LightGrey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="omni/DarkGrey">
-    <color rgba="0.4 0.4 0.4 1"/>
-  </material>
-  <!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
-  <!-- The first use of a macro.  This one was defined in youbot_base/base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
-       that equals "base", and uses it to generate names for its component links and joints
-       (e.g., base_link).  The xacro:included origin block is also an argument to the macro.  By convention,
-       the origin block defines where the component is w.r.t its parent (in this case the parent
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <!-- foot for arm-->
-  <link name="base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-    </inertial>
-  </link>
-  <!-- joint between base_link and arm_0_link -->
-  <joint name="arm_joint_0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="arm_link_0"/>
-  </joint>
-  <link name="arm_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-      <material name="omni/LightGrey"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <mass value="0.2"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_1" type="revolute">
-    <parent link="arm_link_0"/>
-    <child link="arm_link_1"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="arm_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <mass value="0.190421352"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_2" type="revolute">
-    <parent link="arm_link_1"/>
-    <child link="arm_link_2"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-      <material name="omni/Red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <mass value="0.29302326"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_3" type="revolute">
-    <parent link="arm_link_2"/>
-    <child link="arm_link_3"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.471"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_3">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-      <material name="omni/Yellow"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <mass value="0.21931466"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_4" type="revolute">
-    <parent link="arm_link_3"/>
-    <child link="arm_link_4"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.377"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_4">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-      <material name="omni/Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_5" type="revolute">
-    <parent link="arm_link_4"/>
-    <child link="arm_link_5"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.132"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <link name="arm_link_5">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_6" type="fixed">
-    <parent link="arm_link_5"/>
-    <child link="arm_link_6"/>
-    <origin rpy="0 0 0" xyz="0 0 0.248"/>
-  </joint>
-  <link name="arm_link_6">
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="1e-12"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
-    </inertial>
-  </link>
-  <!-- END OF ARM LINKS/JOINTS -->
-</robot>

src/arm_pkg/urdf/arm12.urdf

@@ -1,307 +0,0 @@
-<robot name="robot">
-  <link name="base_footprint"></link>
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint" />
-    <child link="base_link" />
-    <origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
-  </joint>
-  <link name="base_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-      <material name="base_link-material">
-        <color rgba="0 0.6038273388475408 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
-    </inertial>
-  </link>
-  <joint name="Axis_0_Joint" type="revolute">
-    <parent link="base_link" />
-    <child link="Axis_0" />
-    <origin xyz="0 0 0.035" rpy="0 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>  </joint>
-  <link name="Axis_0">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-      <material name="Axis_0-material">
-        <color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_Joint" type="revolute">
-    <parent link="Axis_0" />
-    <child link="Axis_1" />
-    <origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>  </joint>
-  <link name="Axis_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-      <material name="Axis_1-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_to_Segment_1" type="fixed">
-    <parent link="Axis_1" />
-    <child link="Segment_1" />
-    <origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-      <material name="Segment_1-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_Joint" type="revolute">
-    <parent link="Segment_1" />
-    <child link="Axis_2" />
-    <origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>  </joint>
-  <link name="Axis_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-      <material name="Axis_2-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_to_Segment_2" type="fixed">
-    <parent link="Axis_2" />
-    <child link="Segment_2" />
-    <origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-      <material name="Segment_2-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_Joint" type="revolute">
-    <parent link="Segment_2" />
-    <child link="Axis_3" />
-    <origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-      <material name="Axis_3-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_to_Segment_3" type="fixed">
-    <parent link="Axis_3" />
-    <child link="Segment_3" />
-    <origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-      <material name="Segment_3-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Wrist_Joint" type="revolute">
-    <parent link="Segment_3" />
-    <child link="Axis_4" />
-    <origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_4">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-      <material name="Axis_4-material">
-        <color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Continuous_Joint" type="revolute">
-    <parent link="Axis_4" />
-    <child link="Axis_4_C" />
-    <origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
-  </joint>
-  <link name="Axis_4_C">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-      <material name="Axis_4_C-material">
-        <color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_4_C_to_Effector" type="fixed">
-    <parent link="Axis_4_C" />
-    <child link="Effector" />
-    <origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
-  </joint>
-  <link name="Effector">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.05 0.01 0.135" />
-      </geometry>
-      <material name="Effector-material">
-        <color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.01 0.01 0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
-    </inertial>
-  </link>
-</robot>

src/bio_pkg/bio_pkg/bio_node.py

@@ -9,51 +9,56 @@ import time
 import atexit
 import signal
 from std_msgs.msg import String
-from ros2_interfaces_pkg.msg import BioControl
+from astra_msgs.msg import BioControl
-from ros2_interfaces_pkg.msg import BioFeedback
+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.declare_parameter("launch_mode", "bio")
-        self.launch_mode = self.get_parameter('launch_mode').value
+        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.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
-        self.feedback_pub = self.create_publisher(BioFeedback, '/bio/feedback', 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':
+        if self.launch_mode == "anchor":
-            self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10)
+            self.anchor_sub = self.create_subscription(
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+                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")
 
@@ -66,7 +71,9 @@ class SerialRelay(Node):
                     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)
 
@@ -78,11 +85,11 @@ class SerialRelay(Node):
         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,34 +129,48 @@ 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
         ################
@@ -159,33 +179,35 @@ class SerialRelay(Node):
 
         # 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
+if __name__ == "__main__":
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    # 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

@@ -9,7 +9,7 @@
 
   <depend>rclpy</depend>
   <depend>common_interfaces</depend>
-  <depend>ros2_interfaces_pkg</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',
+    version="0.0.0",
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-            ['resource/' + package_name]),
+        ("share/" + package_name, ["package.xml"]),
-        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='tristan',
+    maintainer="tristan",
-    maintainer_email='tristanmcginnis26@gmail.com',
+    maintainer_email="tristanmcginnis26@gmail.com",
-    description='TODO: Package description',
+    description="TODO: Package description",
-    license='TODO: License declaration',
+    license="TODO: License declaration",
     entry_points={
-        'console_scripts': [
+        "console_scripts": ["bio = bio_pkg.bio_node:main"],
-            'bio = bio_pkg.bio_node:main'
-        ],
     },
 )

src/core_pkg/core_pkg/core_headless.py

@@ -13,14 +13,17 @@ 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):
@@ -43,13 +46,15 @@ class Headless(Node):
         # Initialize the gamepad
         self.gamepad = pygame.joystick.Joystick(0)
         self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
+        print(f"Gamepad Found: {self.gamepad.get_name()}")
 
         # Now initialize the ROS2 node
         super().__init__("core_headless")
         self.create_timer(0.15, self.send_controls)
-        self.publisher = self.create_publisher(CoreControl, '/core/control', 10)
+        self.publisher = self.create_publisher(CoreControl, "/core/control", 10)
-        self.lastMsg = String()  # Used to ignore sending controls repeatedly when they do not change
+        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
@@ -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

@@ -19,8 +19,8 @@ from math import copysign, pi
 from std_msgs.msg import String, Header
 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, JointState
 from geometry_msgs.msg import TwistStamped, Twist
-from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback, RevMotorState
+from astra_msgs.msg import CoreControl, CoreFeedback, RevMotorState
-from ros2_interfaces_pkg.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
+from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
 
 serial_pub = None
@@ -38,7 +38,7 @@ control_qos = qos.QoSProfile(
     deadline=Duration(seconds=1),
     lifespan=Duration(nanoseconds=500_000_000),  # 500ms
     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    liveliness_lease_duration=Duration(seconds=5),
 )
 
 # Used to verify the length of an incoming VicCAN feedback message
@@ -52,7 +52,7 @@ viccan_msg_len_dict = {
     53: 4,
     54: 4,
     56: 4,  # really 3, but viccan
-    58: 4   # ditto
+    58: 4,  # ditto
 }
 
 
@@ -62,77 +62,110 @@ class SerialRelay(Node):
         super().__init__("core_node")
 
         # Launch mode -- anchor vs core
-        self.declare_parameter('launch_mode', 'core')
+        self.declare_parameter("launch_mode", "core")
-        self.launch_mode = self.get_parameter('launch_mode').value
+        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':
+        if self.launch_mode == "anchor":
-            self.anchor_fromvic_sub_ = self.create_subscription(VicCAN, '/anchor/from_vic/core', self.relay_fromvic, 20)
+            self.anchor_fromvic_sub_ = self.create_subscription(
-            self.anchor_tovic_pub_ = self.create_publisher(VicCAN, '/anchor/to_vic/relay', 20)
+                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_sub = self.create_subscription(
-            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
+                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, 1)
+        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.control_state_sub_ = self.create_subscription(
-        self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+            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', qos_profile=qos.qos_profile_sensor_data)
+        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)
+        self.joint_state_pub_ = self.create_publisher(
+            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
+        )
         # IMU (embedded BNO-055)
-        self.imu_pub_ = self.create_publisher(Imu, '/core/imu', qos_profile=qos.qos_profile_sensor_data)
+        self.imu_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', qos_profile=qos.qos_profile_sensor_data)
+        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', qos_profile=qos.qos_profile_sensor_data)
+        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.debug_pub = self.create_publisher(String, "/core/debug", 10)
-        self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
+        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()
@@ -141,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
 
@@ -166,7 +199,6 @@ class SerialRelay(Node):
             atexit.register(self.cleanup)
         # end __init__()
 
-
     def run(self):
         # This thread makes all the update processes run in the background
         global thread
@@ -175,7 +207,7 @@ class SerialRelay(Node):
 
         try:
             while rclpy.ok():
-                if self.launch_mode == 'core':
+                if self.launch_mode == "core":
                     self.read_MCU()  # Check the MCU for updates
         except KeyboardInterrupt:
             sys.exit(0)
@@ -213,8 +245,8 @@ class SerialRelay(Node):
     def scale_duty(self, value: float, max_speed: float):
         leftMin = -1
         leftMax = 1
-        rightMin = -max_speed/100.0
+        rightMin = -max_speed / 100.0
-        rightMax = max_speed/100.0
+        rightMax = max_speed / 100.0
 
         # Figure out how 'wide' each range is
         leftSpan = leftMax - leftMin
@@ -227,17 +259,29 @@ class SerialRelay(Node):
         return str(rightMin + (valueScaled * rightSpan))
 
     def send_controls(self, msg: CoreControl):
-        if(msg.turn_to_enable):
+        if msg.turn_to_enable:
-            command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' 
+            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
@@ -255,13 +299,15 @@ class SerialRelay(Node):
         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
@@ -272,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_left = map_range(
-        duty_right = map_range(duty_right, -1, 1, -self.twist_max_duty, self.twist_max_duty)
+            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])
 
@@ -285,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':
+        if self.launch_mode == "anchor":
-            #self.get_logger().info(f"[Core to Anchor Relay] {msg}")
+            # self.get_logger().info(f"[Core to Anchor Relay] {msg}")
-            output = String()#Convert to std_msg string
+            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(
+        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
+                data=data,
-        ))
+            )
-
+        )
 
     def anchor_feedback(self, msg: String):
         output = msg.data
@@ -366,7 +416,7 @@ 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
@@ -376,7 +426,9 @@ class SerialRelay(Node):
         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)})")
+                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:
@@ -386,7 +438,11 @@ class SerialRelay(Node):
             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.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)
@@ -402,7 +458,7 @@ class SerialRelay(Node):
                 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)
+                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]
@@ -427,7 +483,9 @@ class SerialRelay(Node):
                     case 4:
                         motor = self.feedback_new_state.br_motor
                     case _:
-                        self.get_logger().warning(f"Ignoring REV motor feedback 53 with invalid motorId {motorId}")
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 53 with invalid motorId {motorId}"
+                        )
                         return
 
                 if motor:
@@ -455,9 +513,15 @@ class SerialRelay(Node):
                 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 = (
-                joint_state_msg.position = [position * (2 * pi) / CORE_GEAR_RATIO]  # revolutions to radians
+                    JointState()
-                joint_state_msg.velocity = [velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO]  # RPM to rad/s
+                )  # 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
 
@@ -475,7 +539,9 @@ class SerialRelay(Node):
                         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}")
+                        self.get_logger().warning(
+                            f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
+                        )
                         return
 
                 joint_state_msg.header.stamp = msg.header.stamp
@@ -483,15 +549,13 @@ class SerialRelay(Node):
             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*")
@@ -510,7 +574,10 @@ 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
 
 
@@ -523,7 +590,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
+if __name__ == "__main__":
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+    # 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,37 +18,43 @@ 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_ip", "192.168.1.9")
-        self.declare_parameter('camera_port', 37260)
+        self.declare_parameter("camera_port", 37260)
 
         # Get parameters
-        self.camera_ip = self.get_parameter('camera_ip').value
+        self.camera_ip = self.get_parameter("camera_ip").value
-        self.camera_port = self.get_parameter('camera_port').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.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.feedback_pub = self.create_publisher(PtzFeedback, "/ptz/feedback", 10)
-        self.debug_pub = self.create_publisher(String, '/ptz/debug', 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)
@@ -57,7 +63,9 @@ class PtzNode(Node):
 
         # 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
@@ -86,8 +94,7 @@ class PtzNode(Node):
 
             # Request attitude data stream
             await self.camera.send_data_stream_request(
-                DataStreamType.ATTITUDE_DATA,
+                DataStreamType.ATTITUDE_DATA, DataStreamFrequency.HZ_10
-                DataStreamFrequency.HZ_10
             )
 
             # Update connection status
@@ -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 (
-            if cmd_id == CommandID.ATTITUDE_DATA_RESPONSE and isinstance(data, AttitudeData):
+            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
@@ -130,7 +141,6 @@ class PtzNode(Node):
                     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:
@@ -140,7 +150,9 @@ class PtzNode(Node):
                     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(
@@ -152,7 +164,9 @@ class PtzNode(Node):
         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)"
@@ -162,18 +176,19 @@ class PtzNode(Node):
         """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")
+            self.get_logger().warning(
+                "Camera object not initialized, ignoring control command"
+            )
             return
 
-        self.thread_pool.submit(
+        self.thread_pool.submit(self.run_async_func, self.process_control_command(msg))
-            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.")
+            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.
@@ -186,27 +201,35 @@ class PtzNode(Node):
             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:
                 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 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)
@@ -214,9 +237,13 @@ class PtzNode(Node):
                         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")
+                        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}")
@@ -231,7 +258,9 @@ class PtzNode(Node):
     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)
 
@@ -239,18 +268,23 @@ class PtzNode(Node):
         """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
@@ -259,8 +293,7 @@ class PtzNode(Node):
             try:
                 self.get_logger().info("Disabling data stream...")
                 await self.camera.send_data_stream_request(
-                    DataStreamType.ATTITUDE_DATA, 
+                    DataStreamType.ATTITUDE_DATA, DataStreamFrequency.DISABLE
-                    DataStreamFrequency.DISABLE
                 )
                 await asyncio.sleep(0.1)
 
@@ -287,10 +320,14 @@ 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 = 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)
@@ -301,7 +338,9 @@ class PtzNode(Node):
 
             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):
@@ -312,6 +351,7 @@ def main(args=None):
 
     asyncio_thread = None
     if ptz_node.loop:
+
         def run_event_loop(loop):
             asyncio.set_event_loop(loop)
             try:
@@ -324,9 +364,7 @@ def main(args=None):
                     loop.close()
 
         asyncio_thread = threading.Thread(
-            target=run_event_loop, 
+            target=run_event_loop, args=(ptz_node.loop,), daemon=True
-            args=(ptz_node.loop,),
-            daemon=True 
         )
         asyncio_thread.start()
 
@@ -351,5 +389,5 @@ def main(args=None):
         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__(
+    def __init__(self, ip: str, port: int = 37260, *, heartbeat_interval: int = 2):
-        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.reader, self.writer = await asyncio.open_connection(self.ip, self.port)
-                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(
+    def _build_packet_header(self, cmd_id: CommandID, data_len: int) -> bytearray:
-        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(
+        packet = self._build_packet_header(CommandID.ROTATION_CONTROL, data_len)
-            CommandID.ROTATION_CONTROL, data_len
-        )
         packet.extend(struct.pack("bb", turn_yaw, turn_pitch))
         return self._finalize_packet(packet)
 
-    async def send_rotation_command(
+    async def send_rotation_command(self, turn_yaw: int, turn_pitch: int) -> None:
-        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(
+    def _build_attitude_angles_packet(self, yaw: float, pitch: float) -> bytes:
-        self, yaw: float, pitch: float
-    ) -> bytes:
         data_len = 4
-        packet = self._build_packet_header(
+        packet = self._build_packet_header(CommandID.ATTITUDE_ANGLES, data_len)
-            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(
+    async def send_attitude_angles_command(self, yaw: float, pitch: float) -> None:
-        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(
+        logger.debug(f"Sent attitude angles command with yaw {yaw}° and pitch {pitch}°")
-            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(
+        packet = self._build_packet_header(CommandID.SINGLE_AXIS_CONTROL, data_len)
-            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(
+        packet = self._build_packet_header(CommandID.DATA_STREAM_REQUEST, data_len)
-            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."
             )
@@ -374,10 +354,7 @@ class SiyiGimbalCamera:
                         self._data_callback(cmd_id_int, data)
                     continue
 
-                if (
+                if cmd_id_enum == CommandID.ATTITUDE_DATA_RESPONSE and len(data) == 12:
-                    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(
+                        logger.exception(f"Failed to parse attitude data: {e}")
-                            f"Failed to parse attitude data: {e}"
-                        )
                         if self._data_callback:
                             self._data_callback(cmd_id_enum, data)
                 else:
@@ -429,7 +404,9 @@ async def main_sdk_test(): # Renamed to avoid conflict if this file is imported
     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}"
             )
@@ -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

@@ -11,7 +11,7 @@
   <depend>common_interfaces</depend>
   <depend>python3-scipy</depend>
   <depend>python-crccheck-pip</depend>
-  <depend>ros2_interfaces_pkg</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',
+    version="0.0.0",
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages',
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-            ['resource/' + package_name]),
+        ("share/" + package_name, ["package.xml"]),
-        ('share/' + package_name, ['package.xml']),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='tristan',
+    maintainer="tristan",
-    maintainer_email='tristanmcginnis26@gmail.com',
+    maintainer_email="tristanmcginnis26@gmail.com",
-    description='Core rover control package to handle command interpretation and embedded interfacing.',
+    description="Core rover control package to handle command interpretation and embedded interfacing.",
-    license='All Rights Reserved',
+    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/headless_pkg/package.xml

@@ -10,7 +10,7 @@
   <depend>rclpy</depend>
   <depend>common_interfaces</depend>
   <depend>python3-pygame</depend>
-  <depend>ros2_interfaces_pkg</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',
+    version="1.0.0",
-    packages=find_packages(exclude=['test']),
+    packages=find_packages(exclude=["test"]),
     data_files=[
-        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ("share/ament_index/resource_index/packages", ["resource/" + package_name]),
-        ('share/' + package_name, ['package.xml']),
+        ("share/" + package_name, ["package.xml"]),
     ],
-    install_requires=['setuptools'],
+    install_requires=["setuptools"],
     zip_safe=True,
-    maintainer='David Sharpe',
+    maintainer="David Sharpe",
-    maintainer_email='ds0196@uah.edu',
+    maintainer_email="ds0196@uah.edu",
-    description='Headless rover control package to handle command interpretation and embedded interfacing.',
+    description="Headless rover control package to handle command interpretation and embedded interfacing.",
-    license='All Rights Reserved',
+    license="All Rights Reserved",
     entry_points={
-        'console_scripts': [
+        "console_scripts": [
             "headless_full = src.headless_node:main",
         ],
     },

src/headless_pkg/src/headless_node.py

@@ -15,13 +15,15 @@ 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 astra_msgs.msg import CoreControl, ArmManual, BioControl
-from ros2_interfaces_pkg.msg import CoreCtrlState
+from astra_msgs.msg import CoreCtrlState
 
 import pygame
 
 os.environ["SDL_VIDEODRIVER"] = "dummy"  # Prevents pygame from trying to open a display
-os.environ["SDL_AUDIODRIVER"] = "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+os.environ["SDL_AUDIODRIVER"] = (
+    "dummy"  # Force pygame to use a dummy audio driver before pygame.init()
+)
 
 
 CORE_STOP_MSG = CoreControl()  # All zeros by default
@@ -37,7 +39,7 @@ control_qos = qos.QoSProfile(
     deadline=Duration(seconds=1),
     lifespan=Duration(nanoseconds=500_000_000),  # 500ms
     liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5)
+    liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"
@@ -51,11 +53,11 @@ class Headless(Node):
         super().__init__("headless")
 
         # Wait for anchor to start
-        pub_info = self.get_publishers_info_by_topic('/anchor/from_vic/debug')
+        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')
+            pub_info = self.get_publishers_info_by_topic("/anchor/from_vic/debug")
 
         # Wait for a gamepad to be connected
         print("Waiting for gamepad connection...")
@@ -71,17 +73,20 @@ class Headless(Node):
         # Initialize the gamepad
         self.gamepad = pygame.joystick.Joystick(0)
         self.gamepad.init()
-        print(f'Gamepad Found: {self.gamepad.get_name()}')
+        print(f"Gamepad Found: {self.gamepad.get_name()}")
 
         self.create_timer(0.15, self.send_controls)
 
-        self.core_publisher = self.create_publisher(CoreControl, '/core/control', 2)
+        self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
-        self.arm_publisher = self.create_publisher(ArmManual, '/arm/control/manual', 2)
+        self.arm_publisher = self.create_publisher(ArmManual, "/arm/control/manual", 2)
-        self.bio_publisher = self.create_publisher(BioControl, '/bio/control', 2)
+        self.bio_publisher = self.create_publisher(BioControl, "/bio/control", 2)
-
-        self.core_twist_pub_ = self.create_publisher(Twist, '/core/twist', qos_profile=control_qos)
-        self.core_state_pub_ = self.create_publisher(CoreCtrlState, '/core/control/state', qos_profile=control_qos)
 
+        self.core_twist_pub_ = self.create_publisher(
+            Twist, "/core/twist", qos_profile=control_qos
+        )
+        self.core_state_pub_ = self.create_publisher(
+            CoreCtrlState, "/core/control/state", qos_profile=control_qos
+        )
 
         self.ctrl_mode = "core"  # Start in core mode
         self.core_brake_mode = False
@@ -90,7 +95,6 @@ class Headless(Node):
         # Rumble when node is ready (returns False if rumble not supported)
         self.gamepad.rumble(0.7, 0.8, 150)
 
-
     def run(self):
         # This thread makes all the update processes run in the background
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
@@ -104,7 +108,7 @@ class Headless(Node):
             sys.exit(0)
 
     def send_controls(self):
-        """ Read the gamepad state and publish control messages """
+        """Read the gamepad state and publish control messages"""
         for event in pygame.event.get():
             if event.type == pygame.QUIT:
                 pygame.quit()
@@ -122,7 +126,6 @@ class Headless(Node):
             pygame.quit()
             sys.exit(0)
 
-
         new_ctrl_mode = self.ctrl_mode  # if "" then inequality will always be true
 
         # Check for control mode change
@@ -137,7 +140,6 @@ class Headless(Node):
             self.ctrl_mode = new_ctrl_mode
             self.get_logger().info(f"Switched to {self.ctrl_mode} control mode")
 
-
         # CORE
         if self.ctrl_mode == "core" and CORE_MODE == "duty":
             input = CoreControl()
@@ -148,7 +150,6 @@ class Headless(Node):
             right_stick_y = deadzone(self.gamepad.get_axis(4))
             right_trigger = deadzone(self.gamepad.get_axis(5))
 
-
             # Right wheels
             input.right_stick = float(round(-1 * right_stick_y, 2))
 
@@ -158,9 +159,8 @@ 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)
@@ -179,13 +179,17 @@ class Headless(Node):
 
             # Forward/back and Turn
             input.linear.x = -1.0 * left_stick_y
-            input.angular.z = -1.0 * copysign(right_stick_x ** 2, right_stick_x)  # 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
@@ -198,15 +202,19 @@ class Headless(Node):
                 new_max_duty = 0.5
 
             # Only publish if needed
-            if new_brake_mode != self.core_brake_mode or new_max_duty != self.core_max_duty:
+            if (
+                new_brake_mode != self.core_brake_mode
+                or new_max_duty != self.core_max_duty
+            ):
                 self.core_brake_mode = new_brake_mode
                 self.core_max_duty = new_max_duty
                 state_msg = CoreCtrlState()
                 state_msg.brake_mode = bool(self.core_brake_mode)
                 state_msg.max_duty = float(self.core_max_duty)
                 self.core_state_pub_.publish(state_msg)
-                self.get_logger().info(f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}")
+                self.get_logger().info(
-
+                    f"[Core State] Brake: {self.core_brake_mode}, Max Duty: {self.core_max_duty}"
+                )
 
         # ARM and BIO
         if self.ctrl_mode == "arm":
@@ -222,7 +230,6 @@ class Headless(Node):
             right_bumper = self.gamepad.get_button(5)
             dpad_input = self.gamepad.get_hat(0)
 
-
             # EF Grippers
             if left_trigger > 0 and right_trigger > 0:
                 arm_input.gripper = 0
@@ -237,7 +244,6 @@ class Headless(Node):
             elif dpad_input[0] == -1:
                 arm_input.axis0 = -1
 
-
             if right_bumper:  # Control end effector
 
                 # Effector yaw
@@ -255,27 +261,28 @@ class Headless(Node):
             else:  # Control arm axis
 
                 # Axis 1
-                if abs(left_stick_x) > .15:
+                if abs(left_stick_x) > 0.15:
                     arm_input.axis1 = round(left_stick_x)
 
                 # Axis 2
-                if abs(left_stick_y) > .15:
+                if abs(left_stick_y) > 0.15:
                     arm_input.axis2 = -1 * round(left_stick_y)
 
                 # Axis 3
-                if abs(right_stick_y) > .15:
+                if abs(right_stick_y) > 0.15:
                     arm_input.axis3 = -1 * round(right_stick_y)
 
-
             # BIO
             bio_input = BioControl(
                 bio_arm=int(left_stick_y * -100),
-                drill_arm=int(round(right_stick_y) * -100)
+                drill_arm=int(round(right_stick_y) * -100),
             )
 
             # Drill motor (FAERIE)
             if deadzone(left_trigger) > 0 or deadzone(right_trigger) > 0:
-                bio_input.drill = int(30 * (right_trigger - left_trigger))  # Max duty cycle 30%
+                bio_input.drill = int(
+                    30 * (right_trigger - left_trigger)
+                )  # Max duty cycle 30%
 
             self.core_publisher.publish(CORE_STOP_MSG)
             self.arm_publisher.publish(arm_input)
@@ -283,7 +290,7 @@ class Headless(Node):
 
 
 def deadzone(value: float, threshold=0.05) -> float:
-    """ Apply a deadzone to a joystick input so the motors don't sound angry """
+    """Apply a deadzone to a joystick input so the motors don't sound angry"""
     if abs(value) < threshold:
         return 0
     return value
@@ -295,6 +302,9 @@ def main(args=None):
     rclpy.spin(node)
     rclpy.shutdown()
 
-if __name__ == '__main__':
+
-    signal.signal(signal.SIGTERM, lambda signum, frame: sys.exit(0))  # Catch termination signals and exit cleanly
+if __name__ == "__main__":
+    signal.signal(
+        signal.SIGTERM, lambda signum, frame: sys.exit(0)
+    )  # Catch termination signals and exit cleanly
     main()

src/moveit_servo/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(moveit_servo)
+
+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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # 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/moveit_servo/config/kinematics.yaml

@@ -0,0 +1,4 @@
+panda_arm:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.05

src/moveit_servo/launch/servo.launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from moveit_configs_utils import MoveItConfigsBuilder
+
+def generate_launch_description():
+    moveit_config = (
+        MoveItConfigsBuilder("astra_descriptions")
+        .robot_description(file_path="config/robot.urdf.xacro")
+        .to_moveit_configs()
+    )
+
+    servo_node = Node(
+        package="moveit_servo",
+        executable="servo_node",
+        name="servo_node",
+        parameters=[
+            "config/servo_parameters.yaml",
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+        ],
+        output="screen",
+    )
+
+    return LaunchDescription([servo_node])

src/moveit_servo/package.xml

@@ -0,0 +1,18 @@
+<?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>moveit_servo</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="aaron.m.davis@comcast.net">aaron</maintainer>
+  <license>TODO: License declaration</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>