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

.gitmodules

@@ -1,3 +1,6 @@
 [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

auto_start/auto_start_anchor.sh

@@ -14,8 +14,8 @@ 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 (if we aren't using nix)
+# Source ROS 2 Humble setup script
-command -v ros2 || source /opt/ros/humble/setup.bash
+source /opt/ros/humble/setup.bash
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/auto_start_headless_full.sh

@@ -15,7 +15,7 @@ echo "[INFO] Network interface is up!"
 echo "[INFO] Starting ROS node..."
 
 # Source ROS 2 Humble setup script
-command -v ros2 || source /opt/ros/humble/setup.bash
+source /opt/ros/humble/setup.bash
 
 # Source your workspace setup script
 source $SCRIPT_DIR/../install/setup.bash

auto_start/start_rosbag.sh

@@ -17,7 +17,7 @@ done
 echo "[INFO] Network interface is up!"
 
 
-command -v ros2 || source /opt/ros/humble/setup.bash
+source /opt/ros/humble/setup.bash
 source $ANCHOR_WS/install/setup.bash
 [[ -f $AUTONOMY_WS/install/setup.bash ]] && source $AUTONOMY_WS/install/setup.bash
 

flake.lock

@@ -1,29 +1,5 @@
 {
   "nodes": {
-    "astra-msgs": {
-      "inputs": {
-        "nix-ros-overlay": "nix-ros-overlay",
-        "nixpkgs": [
-          "astra-msgs",
-          "nix-ros-overlay",
-          "nixpkgs"
-        ]
-      },
-      "locked": {
-        "lastModified": 1770693861,
-        "narHash": "sha256-8NBtSsKVYtd+NYt51tJ1EteC0nOTemDUBoRXbDoQAuA=",
-        "owner": "SHC-ASTRA",
-        "repo": "astra_msgs",
-        "rev": "60bbb53085b09fbdb7e848b1dd168d526d9af281",
-        "type": "github"
-      },
-      "original": {
-        "owner": "SHC-ASTRA",
-        "ref": "60bbb53085b09fbdb7e848b1dd168d526d9af281",
-        "repo": "astra_msgs",
-        "type": "github"
-      }
-    },
     "flake-utils": {
       "inputs": {
         "systems": "systems"
@@ -42,87 +18,33 @@
         "type": "github"
       }
     },
-    "flake-utils_2": {
-      "inputs": {
-        "systems": "systems_2"
-      },
-      "locked": {
-        "lastModified": 1731533236,
-        "narHash": "sha256-l0KFg5HjrsfsO/JpG+r7fRrqm12kzFHyUHqHCVpMMbI=",
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "rev": "11707dc2f618dd54ca8739b309ec4fc024de578b",
-        "type": "github"
-      },
-      "original": {
-        "owner": "numtide",
-        "repo": "flake-utils",
-        "type": "github"
-      }
-    },
     "nix-ros-overlay": {
       "inputs": {
         "flake-utils": "flake-utils",
         "nixpkgs": "nixpkgs"
       },
       "locked": {
-        "lastModified": 1770408708,
+        "lastModified": 1761810010,
-        "narHash": "sha256-E7ZQRgGrsiuswgXnG7337ZR5s4SdZlheZjxKOQdVoH8=",
+        "narHash": "sha256-o0wJKW603SiOO373BTgeZaF6nDxegMA/cRrzSC2Cscg=",
         "owner": "lopsided98",
         "repo": "nix-ros-overlay",
-        "rev": "e78ba91032c7f8bdd823fbf43937cbf0f4f09747",
+        "rev": "e277df39e3bc6b372a5138c0bcf10198857c55ab",
         "type": "github"
       },
       "original": {
         "owner": "lopsided98",
-        "ref": "develop",
+        "ref": "master",
-        "repo": "nix-ros-overlay",
-        "type": "github"
-      }
-    },
-    "nix-ros-overlay_2": {
-      "inputs": {
-        "flake-utils": "flake-utils_2",
-        "nixpkgs": "nixpkgs_2"
-      },
-      "locked": {
-        "lastModified": 1770108954,
-        "narHash": "sha256-VBj6bd4LPPSfsZJPa/UPPA92dOs6tmQo0XZKqfz/3W4=",
-        "owner": "lopsided98",
-        "repo": "nix-ros-overlay",
-        "rev": "3d05d46451b376e128a1553e78b8870c75d7753a",
-        "type": "github"
-      },
-      "original": {
-        "owner": "lopsided98",
-        "ref": "develop",
         "repo": "nix-ros-overlay",
         "type": "github"
       }
     },
     "nixpkgs": {
       "locked": {
-        "lastModified": 1759381078,
+        "lastModified": 1744849697,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
+        "narHash": "sha256-S9hqvanPSeRu6R4cw0OhvH1rJ+4/s9xIban9C4ocM/0=",
-        "owner": "NixOS",
-        "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
-        "type": "github"
-      },
-      "original": {
         "owner": "lopsided98",
-        "ref": "nix-ros",
         "repo": "nixpkgs",
-        "type": "github"
+        "rev": "6318f538166fef9f5118d8d78b9b43a04bb049e4",
-      }
-    },
-    "nixpkgs_2": {
-      "locked": {
-        "lastModified": 1759381078,
-        "narHash": "sha256-gTrEEp5gEspIcCOx9PD8kMaF1iEmfBcTbO0Jag2QhQs=",
-        "owner": "NixOS",
-        "repo": "nixpkgs",
-        "rev": "7df7ff7d8e00218376575f0acdcc5d66741351ee",
         "type": "github"
       },
       "original": {
@@ -134,8 +56,7 @@
     },
     "root": {
       "inputs": {
-        "astra-msgs": "astra-msgs",
+        "nix-ros-overlay": "nix-ros-overlay",
-        "nix-ros-overlay": "nix-ros-overlay_2",
         "nixpkgs": [
           "nix-ros-overlay",
           "nixpkgs"
@@ -156,21 +77,6 @@
         "repo": "default",
         "type": "github"
       }
-    },
-    "systems_2": {
-      "locked": {
-        "lastModified": 1681028828,
-        "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=",
-        "owner": "nix-systems",
-        "repo": "default",
-        "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e",
-        "type": "github"
-      },
-      "original": {
-        "owner": "nix-systems",
-        "repo": "default",
-        "type": "github"
-      }
     }
   },
   "root": "root",

flake.nix

@@ -2,10 +2,8 @@
   description = "Development environment for ASTRA Anchor";
 
   inputs = {
-    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/develop";
+    nix-ros-overlay.url = "github:lopsided98/nix-ros-overlay/master";
     nixpkgs.follows = "nix-ros-overlay/nixpkgs"; # IMPORTANT!!!
-    # specify astra_msgs commit hash to the one we support
-    astra-msgs.url = "github:SHC-ASTRA/astra_msgs?ref=60bbb53085b09fbdb7e848b1dd168d526d9af281";
   };
 
   outputs =
@@ -13,7 +11,6 @@
       self,
       nix-ros-overlay,
       nixpkgs,
-      astra-msgs,
     }:
     nix-ros-overlay.inputs.flake-utils.lib.eachDefaultSystem (
       system:
@@ -21,15 +18,20 @@
         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
       {
         devShells.default = pkgs.mkShell {
           name = "ASTRA Anchor";
           packages = with pkgs; [
-            astra-msgs.packages.${system}.astra-msgs
             colcon
-            (python313.withPackages (
+            (python312.withPackages (
               p: with p; [
                 pyserial
                 pygame
@@ -39,11 +41,10 @@
               ]
             ))
             (
-              with rosPackages.humble;
+              with rosPackages.jazzy;
               buildEnv {
                 paths = [
                   ros-core
-                  rqt-graph
                   ros2cli
                   ros2run
                   ros2bag
@@ -61,23 +62,22 @@
                   control-msgs
                   control-toolbox
                   moveit-core
-                  moveit-planners
                   moveit-common
                   moveit-msgs
                   moveit-ros-planning
                   moveit-ros-planning-interface
-		  moveit-ros-visualization
                   moveit-configs-utils
                   moveit-ros-move-group
                   moveit-servo
                   moveit-simple-controller-manager
-                  topic-based-ros2-control
                   pilz-industrial-motion-planner
                   pick-ik
                   ompl
-                  joy
-                  ros2-controllers
                   chomp-motion-planner
+                  joy
+                  ros-gz-sim
+                  ros-gz-bridge
+                  # ros2-controllers nixpkg does not build :(
                 ];
               }
             )
@@ -86,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

@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
 from std_srvs.srv import Empty
 
 import signal
@@ -8,7 +7,6 @@ import time
 import atexit
 
 import serial
-import serial.tools.list_ports
 import os
 import sys
 import threading
@@ -17,15 +15,10 @@ import glob
 from std_msgs.msg import String, Header
 from astra_msgs.msg import VicCAN
 
-KNOWN_USBS = [
+serial_pub = None
-    (0x2E8A, 0x00C0),  # Raspberry Pi Pico
+thread = None
-    (0x1A86, 0x55D4),  # Adafruit Feather ESP32 V2
-    (0x10C4, 0xEA60),  # DOIT ESP32 Devkit V1
-    (0x1A86, 0x55D3),  # ESP32 S3 Development Board
-]
 
 
-class Anchor(Node):
 """
 Publishers:
   * /anchor/from_vic/debug
@@ -46,62 +39,19 @@ class Anchor(Node):
     - 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'
 
-        self.serial_port: str | None = None  # e.g., "/dev/ttyUSB0"
-
-        # Serial port override
-        if port_override := os.getenv("PORT_OVERRIDE"):
-            self.serial_port = port_override
-
-        ##################################################
-        # Serial MCU Discovery
-
-        # If there was not a port override, look for a MCU over USB for Serial.
-        if self.serial_port is None:
-            comports = serial.tools.list_ports.comports()
-            real_ports = list(
-                filter(
-                    lambda p: p.vid is not None
-                    and p.pid is not None
-                    and p.device is not None,
-                    comports,
-                )
-            )
-            recog_ports = list(filter(lambda p: (p.vid, p.pid) in KNOWN_USBS, comports))
-
-            if len(recog_ports) == 1:  # Found singular recognized MCU
-                found_port = recog_ports[0]
-                self.get_logger().info(
-                    f"Selecting MCU '{found_port.description}' at {found_port.device}."
-                )
-                self.serial_port = found_port.device  # String, location of device file; e.g., '/dev/ttyACM0'
-            elif len(recog_ports) > 1:  # Found multiple recognized MCUs
-                # Kinda jank log message
-                self.get_logger().error(
-                    f"Found multiple recognized MCUs: {[p.device for p in recog_ports].__str__()}"
-                )
-                # Don't set self.serial_port; later if-statement will exit()
-            elif (
-                len(recog_ports) == 0 and len(real_ports) > 0
-            ):  # Found real ports but none recognized; i.e. maybe found an IMU or camera but not a MCU
-                self.get_logger().error(
-                    f"No recognized MCUs found; instead found {[p.device for p in real_ports].__str__()}."
-                )
-                # Don't set self.serial_port; later if-statement will exit()
-            else:  # Found jack shit
-                self.get_logger().error("No valid Serial ports specified or found.")
-                # Don't set self.serial_port; later if-statement will exit()
-
-        # We still don't have a serial port; fall back to legacy discovery (Areeb's code)
         # Loop through all serial devices on the computer to check for the MCU
-        if self.serial_port is None:
+        self.port = None
-            self.get_logger().warning("Falling back to legacy MCU discovery...")
+        if port_override := os.getenv("PORT_OVERRIDE"):
-            ports = Anchor.list_serial_ports()
+            self.port = port_override
-            for _ in range(4):
+        ports = SerialRelay.list_serial_ports()
-                if self.serial_port is not None:
+        for i in range(4):
+            if self.port is not None:
                 break
             for port in ports:
                 try:
@@ -113,60 +63,22 @@ class Anchor(Node):
 
                     # if pong is in response, then we are talking with the MCU
                     if b"pong" in response:
-                            self.serial_port = port
+                        self.port = port
-                            self.get_logger().info(f"Found MCU at {self.serial_port}!")
+                        self.get_logger().info(f"Found MCU at {self.port}!")
                         break
                 except:
                     pass
 
-        # If port is still None then we ain't finding no mcu
+        if self.port is None:
-        if self.serial_port is None:
+            self.get_logger().info("Unable to find MCU...")
-            self.get_logger().error("Unable to find MCU. Exiting...")
-            time.sleep(1)
-            sys.exit(1)
-        # Found a Serial port, try to open it; above code has not officially opened a Serial port
-        else:
-            self.get_logger().debug(
-                f"Attempting to open Serial port '{self.serial_port}'..."
-            )
-            try:
-                self.serial_interface = serial.Serial(
-                    self.serial_port, 115200, timeout=1
-                )
-
-                # Attempt to get name of connected MCU
-                self.serial_interface.write(
-                    b"can_relay_mode,on\n"
-                )  # can_relay_ready,[mcu]
-                mcu_name: str = ""
-                for _ in range(4):
-                    response = self.serial_interface.read_until(bytes("\n", "utf8"))
-                    try:
-                        if b"can_relay_ready" in response:
-                            args: list[str] = response.decode("utf8").strip().split(",")
-                            if len(args) == 2:
-                                mcu_name = args[1]
-                            break
-                    except UnicodeDecodeError:
-                        pass  # ignore malformed responses
-                self.get_logger().info(
-                    f"MCU '{mcu_name}' is ready at '{self.serial_port}'."
-                )
-
-            except serial.SerialException as e:
-                self.get_logger().error(
-                    f"Could not open Serial port '{self.serial_port}' for reason:"
-                )
-                self.get_logger().error(e.strerror)
             time.sleep(1)
             sys.exit(1)
 
-        # Close serial port on exit
+        self.ser = serial.Serial(self.port, 115200)
+        self.get_logger().info(f"Enabling Relay Mode")
+        self.ser.write(b"can_relay_mode,on\n")
         atexit.register(self.cleanup)
 
-        ##################################################
-        # ROS2 Topic Setup
-
         # New pub/sub with VicCAN
         self.fromvic_debug_pub_ = self.create_publisher(
             String, "/anchor/from_vic/debug", 20
@@ -203,10 +115,22 @@ class Anchor(Node):
             String, "/anchor/relay", self.on_relay_tovic_string, 10
         )
 
+    def run(self):
+        # This thread makes all the update processes run in the background
+        global thread
+        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
+        thread.start()
+
+        try:
+            while rclpy.ok():
+                self.read_MCU()  # Check the MCU for updates
+        except KeyboardInterrupt:
+            sys.exit(0)
+
     def read_MCU(self):
         """Check the USB serial port for new data from the MCU, and publish string to appropriate topics"""
         try:
-            output = str(self.serial_interface.readline(), "utf8")
+            output = str(self.ser.readline(), "utf8")
 
             if output:
                 self.relay_fromvic(output)
@@ -232,20 +156,14 @@ class Anchor(Node):
         except serial.SerialException as e:
             print(f"SerialException: {e}")
             print("Closing serial port.")
-            try:
+            if self.ser.is_open:
-                if self.serial_interface.is_open:
+                self.ser.close()
-                    self.serial_interface.close()
-            except:
-                pass
             exit(1)
         except TypeError as e:
             print(f"TypeError: {e}")
             print("Closing serial port.")
-            try:
+            if self.ser.is_open:
-                if self.serial_interface.is_open:
+                self.ser.close()
-                    self.serial_interface.close()
-            except:
-                pass
             exit(1)
         except Exception as e:
             print(f"Exception: {e}")
@@ -266,7 +184,7 @@ class Anchor(Node):
             output += f",{round(num, 7)}"  # limit to 7 decimal places
         output += "\n"
         # self.get_logger().info(f"VicCAN relay to MCU: {output}")
-        self.serial_interface.write(bytes(output, "utf8"))
+        self.ser.write(bytes(output, "utf8"))
 
     def relay_fromvic(self, msg: str):
         """Relay a string message from the MCU to the appropriate VicCAN topic"""
@@ -328,7 +246,7 @@ class Anchor(Node):
         """Relay a raw string message to the MCU for debugging"""
         message = msg.data
         # self.get_logger().info(f"Sending command to MCU: {msg}")
-        self.serial_interface.write(bytes(message, "utf8"))
+        self.ser.write(bytes(message, "utf8"))
 
     @staticmethod
     def list_serial_ports():
@@ -336,29 +254,28 @@ class Anchor(Node):
 
     def cleanup(self):
         print("Cleaning up before terminating...")
-        if self.serial_interface.is_open:
+        if self.ser.is_open:
-            self.serial_interface.close()
+            self.ser.close()
+
+
+def myexcepthook(type, value, tb):
+    print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):
-    try:
     rclpy.init(args=args)
-        anchor_node = Anchor()
+    sys.excepthook = myexcepthook
 
-        thread = threading.Thread(target=rclpy.spin, args=(anchor_node,), daemon=True)
+    global serial_pub
-        thread.start()
 
-        rate = anchor_node.create_rate(100)  # 100 Hz -- arbitrary rate
+    serial_pub = SerialRelay()
-        while rclpy.ok():
+    serial_pub.run()
-            anchor_node.read_MCU()  # Check the MCU for updates
-            rate.sleep()
-    except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal, shutting down...")
-    finally:
-        rclpy.try_shutdown()
 
 
 if __name__ == "__main__":
+    # signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0))  # Catch Ctrl+Z and exit cleanly
     signal.signal(
         signal.SIGTERM, lambda signum, frame: sys.exit(0)
     )  # Catch termination signals and exit cleanly

src/arm_pkg/arm_pkg/arm_node.py

@@ -49,7 +49,7 @@ class SerialRelay(Node):
 
         # Create subscribers
         self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 2
+            ArmManual, "/arm/control/manual", self.send_manual, 10
         )
 
         # New messages
@@ -190,7 +190,7 @@ class SerialRelay(Node):
 
         command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
 
-        command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
+        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
         command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 

src/bio_pkg/bio_pkg/bio_node.py

@@ -1,203 +1,241 @@
-import signal
+import rclpy
+from rclpy.node import Node
+import serial
 import sys
 import threading
+import os
+import glob
 import time
-
+import atexit
-import rclpy
+import signal
-from astra_msgs.action import BioVacuum
+from std_msgs.msg import String
-from astra_msgs.msg import BioControl, BioDistributor, VicCAN
+from astra_msgs.msg import BioControl
-from astra_msgs.srv import BioTestTube
+from astra_msgs.msg import BioFeedback
-from rclpy.action import ActionServer
-from rclpy.node import Node
-from std_msgs.msg import Header, String
 
 serial_pub = None
 thread = None
 
-# used to verify the length of an incoming VicCAN feedback message
-# key is VicCAN command_id, value is expected length of data list
-viccan_citadel_msg_len_dict = {
-    # empty because not expecting any VicCAN from citadel atm
-}
-viccan_lance_msg_len_dict = {
-    # empty because not sure what VicCAN commands LANCE sends
-}
-
 
 class SerialRelay(Node):
     def __init__(self):
         # Initialize node
         super().__init__("bio_node")
 
-        # Anchor Topics
+        # Get launch mode parameter
-        self.anchor_fromvic_sub_ = self.create_subscription(
+        self.declare_parameter("launch_mode", "bio")
-            VicCAN, "/anchor/from_vic/bio", self.relay_fromvic, 20
+        self.launch_mode = self.get_parameter("launch_mode").value
-        )
+        self.get_logger().info(f"bio launch_mode is: {self.launch_mode}")
-        self.anchor_tovic_pub_ = self.create_publisher(
+
-            VicCAN, "/anchor/to_vic/relay", 20
+        # Create publishers
+        self.debug_pub = self.create_publisher(String, "/bio/feedback/debug", 10)
+        self.feedback_pub = self.create_publisher(BioFeedback, "/bio/feedback", 10)
+
+        # Create subscribers
+        self.control_sub = self.create_subscription(
+            BioControl, "/bio/control", self.send_control, 10
         )
 
+        # Create a publisher for telemetry
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
+
+        # Topics used in anchor mode
+        if self.launch_mode == "anchor":
             self.anchor_sub = self.create_subscription(
                 String, "/anchor/bio/feedback", self.anchor_feedback, 10
             )
             self.anchor_pub = self.create_publisher(String, "/anchor/relay", 10)
 
-        self.distributor_sub = self.create_subscription(
+        self.bio_feedback = BioFeedback()
-            BioDistributor,
-            "/bio/control/distributor",
-            self.distributor_callback,
-            10,
-        )
 
-        # Services
+        # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
-        self.test_tube_service = self.create_service(
+        if self.launch_mode == "bio":
-            BioTestTube, "/bio/control/test_tube", self.test_tube_callback
+            # 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
                     )
+                    ser = serial.Serial(set_port, 115200, timeout=1)
+                    # print(f"Checking port {port}...")
+                    ser.write(b"ping\n")
+                    response = ser.read_until("\n")
 
-        # Actions
+                    # if pong is in response, then we are talking with the MCU
-        self._action_server = ActionServer(
+                    if b"pong" in response:
-            self, BioVacuum, "/bio/actions/vacuum", self.execute_vacuum
+                        self.port = set_port
+                        self.get_logger().info(f"Found MCU at {set_port}!")
+                        break
+                except:
+                    pass
+
+            if self.port is None:
+                self.get_logger().info(
+                    "Unable to find MCU... please make sure it is connected."
                 )
+                time.sleep(1)
+                sys.exit(1)
+
+            self.ser = serial.Serial(self.port, 115200)
+            atexit.register(self.cleanup)
 
     def run(self):
         global thread
         thread = threading.Thread(target=rclpy.spin, args=(self,), daemon=True)
         thread.start()
 
+        # if in arm mode, will need to read from the MCU
+
         try:
             while rclpy.ok():
+                if self.launch_mode == "bio":
+                    if self.ser.in_waiting:
+                        self.read_mcu()
+                    else:
                         time.sleep(0.1)
         except KeyboardInterrupt:
             pass
+        finally:
+            self.cleanup()
+
+    # Currently will just spit out all values over the /arm/feedback/debug topic as strings
+    def read_mcu(self):
+        try:
+            output = str(self.ser.readline(), "utf8")
+            if output:
+                self.get_logger().info(f"[MCU] {output}")
+                msg = String()
+                msg.data = output
+                self.debug_pub.publish(msg)
+        except serial.SerialException:
+            self.get_logger().info("SerialException caught... closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except TypeError as e:
+            self.get_logger().info(f"TypeError: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+        except Exception as e:
+            print(f"Exception: {e}")
+            print("Closing serial port.")
+            if self.ser.is_open:
+                self.ser.close()
+            pass
+
+    def send_ik(self, msg):
+        pass
 
     def send_control(self, msg: BioControl):
-        print("todo")
+        # 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"
+            )
+            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"
+            )
+            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"
+            )
+            self.send_cmd(command)
+
+        # LSS (SCYTHE)
+        command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n"
+        # self.send_cmd(command)
+
+        # Vibration Motor
+        command += "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n"
+        # self.send_cmd(command)
+
+        # FAERIE Control Commands
+        ################
+
+        # To be reviewed before use#
+
+        # Laser
+        command += "can_relay_tovic,digit,28," + str(msg.laser) + "\n"
+        # self.send_cmd(command)
+
+        # Drill (SCABBARD)
+        command += f"can_relay_tovic,digit,19,{msg.drill:.2f}\n"
+        # 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):
-        # 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
-    def relay_fromvic(self, msg: VicCAN):
+            self.get_logger().info(f"[Bio to MCU] {msg}")
-        # self.get_logger().info(msg)
+            self.ser.write(bytes(msg, "utf8"))
-        if msg.mcu_name == "citadel":
-            self.process_fromvic_citadel(msg)
-
-    def process_fromvic_citadel(self, msg: VicCAN):
-        # Check message len to prevent crashing on bad data
-        if msg.command_id in viccan_citadel_msg_len_dict:
-            expected_len = viccan_citadel_msg_len_dict[msg.command_id]
-            if len(msg.data) != expected_len:
-                self.get_logger().warning(
-                    f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})"
-                )
-            return
 
     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}")
-        # no data planned to be received from citadel, not sure about lance
 
-    def distributor_callback(self, msg: BioDistributor):
+        if output.startswith(
-        distributor_arr = msg.distributor_id
+            "can_relay_fromvic,citadel,54"
-        vic_cmd = VicCAN(
+        ):  # bat, 12, 5, Voltage readings * 100
-            header=Header(stamp=self.get_clock().now().to_msg()),
+            self.bio_feedback.bat_voltage = float(parts[3]) / 100.0
-            mcu_name="citadel",
+            self.bio_feedback.voltage_12 = float(parts[4]) / 100.0
-            command_id=40,
+            self.bio_feedback.voltage_5 = float(parts[5]) / 100.0
-            data=[
+        elif output.startswith("can_relay_fromvic,digit,57"):
-                clamp_short(distributor_arr[0]),
+            self.bio_feedback.drill_temp = float(parts[3])
-                clamp_short(distributor_arr[1]),
+            self.bio_feedback.drill_humidity = float(parts[4])
-                clamp_short(distributor_arr[2]),
-                0,
-            ],
-        )
-        self.anchor_tovic_pub_.publish(vic_cmd)
 
-    def test_tube_callback(self, request, response):
+    def publish_feedback(self):
-        tubes_cmd = VicCAN(
+        self.feedback_pub.publish(self.bio_feedback)
-            header=Header(stamp=self.get_clock().now().to_msg()),
-            mcu_name="citadel",
-            command_id=40,
-            data=[
-                float(int.from_bytes(request.tube_id)),
-                float(request.milliliters),
-            ],
-        )
-        self.anchor_tovic_pub_.publish(tubes_cmd)
-        return response
 
-    def execute_vacuum(self, goal_handle):
+    @staticmethod
-        valve_id = int.from_bytes(goal_handle.request.valve_id)
+    def list_serial_ports():
-        duty = int.from_bytes(goal_handle.request.fan_duty_cycle_percent)
+        return glob.glob("/dev/ttyUSB*") + glob.glob("/dev/ttyACM*")
-        total = goal_handle.request.fan_time_ms
+        # return glob.glob("/dev/tty[A-Za-z]*")
 
-        # open valve
+    def cleanup(self):
-        self.anchor_tovic_pub_.publish(
+        print("Cleaning up...")
-            VicCAN(
+        try:
-                header=Header(stamp=self.get_clock().now().to_msg()),
+            if self.ser.is_open:
-                mcu_name="citadel",
+                self.ser.close()
-                command_id=40,
+        except Exception as e:
-                data=[float(valve_id)],
+            exit(0)
-            )
-        )
-
-        # set fan duty cycle
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[float(duty)],
-            )
-        )
-
-        feedback = BioVacuum.Feedback()
-        start = time.time()
-
-        while True:
-            elapsed = int((time.time() - start) * 1000)
-            remaining = max(0, total - elapsed)
-
-            feedback.fan_time_remaining_ms = remaining
-            goal_handle.publish_feedback(feedback)
-
-            if remaining == 0:
-                break
-
-            time.sleep(0.1)
-
-        # stop fan
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=19,
-                data=[0.0],
-            )
-        )
-
-        # close valve
-        self.anchor_tovic_pub_.publish(
-            VicCAN(
-                header=Header(stamp=self.get_clock().now().to_msg()),
-                mcu_name="citadel",
-                command_id=40,
-                data=[-1.0],
-            )
-        )
-
-        goal_handle.succeed()
-        return BioVacuum.Result()
-
-
-def clamp_short(x: int) -> int:
-    return max(-32768, min(32767, x))
 
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)
+    if serial_pub:
+        serial_pub.cleanup()
 
 
 def main(args=None):

src/core_pkg/core_pkg/core_node.py

@@ -31,14 +31,14 @@ CORE_WHEEL_RADIUS = 0.171  # meters
 CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
 
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    # deadline=Duration(seconds=1),
+    deadline=Duration(seconds=1),
-    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    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

src/headless_pkg/src/headless_node.py

@@ -1,6 +1,5 @@
 import rclpy
 from rclpy.node import Node
-from rclpy.executors import ExternalShutdownException
 from rclpy import qos
 from rclpy.duration import Duration
 
@@ -12,8 +11,6 @@ import os
 import sys
 import threading
 import glob
-import pwd
-import grp
 from math import copysign
 
 from std_msgs.msg import String
@@ -35,14 +32,14 @@ ARM_STOP_MSG = ArmManual()  # "
 BIO_STOP_MSG = BioControl()  # "
 
 control_qos = qos.QoSProfile(
-    # history=qos.QoSHistoryPolicy.KEEP_LAST,
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    # durability=qos.QoSDurabilityPolicy.VOLATILE,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
-    # deadline=Duration(seconds=1),
+    deadline=Duration(seconds=1),
-    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    # liveliness_lease_duration=Duration(seconds=5),
+    liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"
@@ -74,37 +71,10 @@ class Headless(Node):
             print("No gamepad found. Waiting...")
 
         # Initialize the gamepad
-        id = 0
+        self.gamepad = pygame.joystick.Joystick(0)
-        while True:
-            self.num_gamepads = pygame.joystick.get_count()
-            if id >= self.num_gamepads:
-                self.get_logger().fatal("Ran out of controllers to try")
-                sys.exit(1)
-
-            try:
-                self.gamepad = pygame.joystick.Joystick(id)
         self.gamepad.init()
-            except Exception as e:
-                self.get_logger().error("Error when initializing gamepad")
-                self.get_logger().error(e)
-                id += 1
-                continue
         print(f"Gamepad Found: {self.gamepad.get_name()}")
 
-            if self.gamepad.get_numhats() == 0 or self.gamepad.get_numaxes() < 5:
-                self.get_logger().error("Controller not correctly initialized.")
-                if not is_user_in_group("input"):
-                    self.get_logger().warning(
-                        "If using NixOS, you may need to add yourself to the 'input' group."
-                    )
-                if is_user_in_group("plugdev"):
-                    self.get_logger().warning(
-                        "If using NixOS, you may need to remove yourself from the 'plugdev' group."
-                    )
-            else:
-                break
-            id += 1
-
         self.create_timer(0.15, self.send_controls)
 
         self.core_publisher = self.create_publisher(CoreControl, "/core/control", 2)
@@ -145,7 +115,7 @@ class Headless(Node):
                 sys.exit(0)
 
         # Check if controller is still connected
-        if pygame.joystick.get_count() != self.num_gamepads:
+        if pygame.joystick.get_count() == 0:
             print("Gamepad disconnected. Exiting...")
             # Send one last zero control message
             self.core_publisher.publish(CORE_STOP_MSG)
@@ -326,33 +296,10 @@ def deadzone(value: float, threshold=0.05) -> float:
     return value
 
 
-def is_user_in_group(group_name: str) -> bool:
-    # Copied from https://zetcode.com/python/os-getgrouplist/
-    try:
-        username = os.getlogin()
-
-        # Get group ID from name
-        group_info = grp.getgrnam(group_name)
-        target_gid = group_info.gr_gid
-
-        # Get user's groups
-        user_info = pwd.getpwnam(username)
-        user_groups = os.getgrouplist(username, user_info.pw_gid)
-
-        return target_gid in user_groups
-    except KeyError:
-        return False
-
-
 def main(args=None):
-    try:
     rclpy.init(args=args)
-
     node = Headless()
     rclpy.spin(node)
-    except (KeyboardInterrupt, ExternalShutdownException):
-        print("Caught shutdown signal. Exiting...")
-    finally:
     rclpy.shutdown()
 
 

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>

src/servo_arm_twist_pkg/CMakeLists.txt

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

src/servo_arm_twist_pkg/README.md

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

src/servo_arm_twist_pkg/package.xml

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

src/servo_arm_twist_pkg/src/joystick_twist.cpp

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