Merge pull request #20 from SHC-ASTRA/new_ik

Integrate Moveit2, remove ikpy
2026-02-11 09:20:40 +00:00 · 2025-10-25 11:49:43 -05:00
parent 1281236b36 90e2aa5070
commit f735f7194e
19 changed files with 591 additions and 915 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -14,3 +14,4 @@ __pycache__/

 #Direnv
 .direnv/
+.venv
--- a/auto_start/auto_start_anchor.sh
+++ b/auto_start/auto_start_anchor.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
--- a/auto_start/auto_start_core_headless.sh
+++ b/auto_start/auto_start_core_headless.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
--- a/auto_start/auto_start_headless_full.sh
+++ b/auto_start/auto_start_headless_full.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash

 # Wait for a network interface to be up (not necessarily online)
 while ! ip link show | grep -q "state UP"; do
--- a/auto_start/start_rosbag.sh
+++ b/auto_start/start_rosbag.sh
@@ -1,4 +1,4 @@
-#!/bin/bash
+#!/usr/bin/env bash

 ANCHOR_WS="/home/clucky/rover-ros2"
 AUTONOMY_WS="/home/clucky/rover-Autonomy"
--- a/flake.nix
+++ b/flake.nix
@@ -53,6 +53,23 @@
                  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
+                  topic-based-ros2-control
+                  pilz-industrial-motion-planner
+                  pick-ik
+                  ompl
+                  chomp-motion-planner
+                  joy
                  # ros2-controllers nixpkg does not build :(
                ];
              }
--- a/src/anchor_pkg/launch/rover.launch.py
+++ b/src/anchor_pkg/launch/rover.launch.py
@@ -2,11 +2,10 @@

 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
 from sys import dont_write_bytecode
 dont_write_bytecode = True
@@ -18,6 +17,7 @@ def launch_setup(context, *args, **kwargs):

    if mode == 'anchor':
        # Launch every node and pass "anchor" as the parameter
+
        nodes.append(
            Node(
                package='arm_pkg',
--- a/src/arm_pkg/arm_pkg/arm_headless.py
+++ b/src/arm_pkg/arm_pkg/arm_headless.py
@@ -17,9 +17,12 @@ from ros2_interfaces_pkg.msg import ArmManual
 from ros2_interfaces_pkg.msg import ArmIK


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

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

        self.create_timer(0.1, self.send_manual)

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

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

        # Create a subscriber to listen to any commands sent for the pico

        # Depricated, kept temporarily for reference
-        #self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)
-
-        #self.debug_sub = self.create_subscription(String, '/arm/feedback/debug', self.read_feedback, 10)
+        # self.subscriber = self.create_subscription(String, '/astra/arm/feedback', self.read_feedback, 10)

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

        self.laser_status = 0

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

-
    def run(self):
        # This thread makes all the update processes run in the background
        thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
        thread.start()

-
        try:
            while rclpy.ok():
-                #Check the pico for updates
+                # Check the pico for updates

-                #self.read_feedback()
-                if pygame.joystick.get_count() == 0: #if controller disconnected, wait for it to be reconnected
+                # self.read_feedback()
+                if (
+                    pygame.joystick.get_count() == 0
+                ):  # if controller disconnected, wait for it to be reconnected
                    print(f"Gamepad disconnected: {self.gamepad.get_name()}")

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

-
        except KeyboardInterrupt:
            sys.exit(0)

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

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

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

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

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

-        else:   # Control arm axis
+        else:  # Control arm axis
            dpad_input = self.gamepad.get_hat(0)
            input.axis0 = 0
            if dpad_input[0] == 1:
@@ -141,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
-        #axis2 -> LT
-        #axis5 -> RT
-        #Buttons0 -> A
-        #Buttons1 -> B
-        #Buttons2 -> X
-        #Buttons3 -> Y
-        #Buttons4 -> LB
-        #Buttons5 -> RB
-        #Buttons6 -> Back
-        #Buttons7 -> Start
+        # Button Mappings
+        # axis2 -> LT
+        # axis5 -> RT
+        # Buttons0 -> A
+        # Buttons1 -> B
+        # Buttons2 -> X
+        # Buttons3 -> Y
+        # Buttons4 -> LB
+        # Buttons5 -> RB
+        # Buttons6 -> Back
+        # Buttons7 -> Start

        input.linear_actuator = 0

-
        if pygame.joystick.get_count() != 0:

-            self.get_logger().info(f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n")
+            self.get_logger().info(
+                f"[Ctrl] {input.axis0}, {input.axis1}, {input.axis2}, {input.axis3}\n"
+            )
            self.manual_pub.publish(input)
        else:
            pass
@@ -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()
-    #node.run()
+    # tb_bs = BaseStation()
+    # node.run()


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

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

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

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

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

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

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

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

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

@@ -98,7 +105,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 +119,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 +133,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 +160,19 @@ class SerialRelay(Node):
                self.ser.close()
            pass

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

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

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

-        #Send controls for arm
-        command = "can_relay_tovic,arm,18," + str(int(msg.brake)) + "\n"
-        command += "can_relay_tovic,arm,39," + str(axis0) + "," + str(axis1) + "," + str(axis2) + "," + str(axis3) + "\n"
+        # Send controls for arm
+        command = f"can_relay_tovic,arm,18,{int(msg.brake)}\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 += "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 += f"can_relay_tovic,digit,39,{msg.effector_yaw},{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 +233,8 @@ 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 +252,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
-            # tempMsg = String()
-            # tempMsg.data = "Angles: " + str(angles)
-            # #self.debug_pub.publish(tempMsg)
+
+            # Joint state publisher for URDF visualization
+            self.joint_state.position[0] = math.radians(angles[0])  # Axis 0
+            self.joint_state.position[1] = math.radians(angles[1])  # Axis 1
+            self.joint_state.position[2] = math.radians(-angles[2])  # Axis 2 (inverted)
+            self.joint_state.position[3] = math.radians(-angles[3])  # Axis 3 (inverted)
+            # Wrist is handled by digit feedback
+            self.joint_state.header.stamp = self.get_clock().now().to_msg()
+            self.joint_state_pub.publish(self.joint_state)
+
        else:
            self.get_logger().info("Invalid angle feedback input format")

@@ -364,11 +307,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 +320,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 +335,10 @@ def main(args=None):
    serial_pub = SerialRelay()
    serial_pub.run()

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

 setup(
    name=package_name,
-    version='0.0.0',
+    version='1.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
-        ('share/' + package_name, ['package.xml']),
-        (os.path.join('share', package_name, 'launch'), glob('launch/*')),
-  	    (os.path.join('share', package_name, 'urdf'), glob('urdf/*')),
+        ('share/' + package_name, ['package.xml'])
    ],
    install_requires=['setuptools'],
    zip_safe=True,
--- a/src/arm_pkg/urdf/arm11.urdf
+++ b/src/arm_pkg/urdf/arm11.urdf
@@ -1,239 +0,0 @@
-<?xml version="1.0" ?>
-<!-- =================================================================================== -->
-<!-- |    This document was autogenerated by xacro from omni_description/robots/omnipointer_arm_only.urdf.xacro | -->
-<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
-<!-- =================================================================================== -->
-<robot name="omnipointer" xmlns:xacro="http://ros.org/wiki/xacro">
-  <!--MX64trntbl + MX106:  ? + 0.153 -->
-  <!-- singleaxismount + 2.5girder + singleaxismount + base + MX106: 0.007370876 + 0.0226796 + 0.007370876 + ? + 0.153 -->
-  <!-- frame106 + singleaxismount + adjustgirder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.110563 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 5girder + singleaxismount + base + MX28: 0.0907185 + 0.00737088 + 0.0368544 + 0.00737088 + ? + 0.077 -->
-  <!-- frame28 + singleaxismount + 2.5girder + singleaxismount + tip: 0.0907185 + 0.00737088 + 0.0226796 + 0.00737088 + ? -->
-  <material name="omni/Blue">
-    <color rgba="0 0 0.8 1"/>
-  </material>
-  <material name="omni/Red">
-    <color rgba="1 0 0 1"/>
-  </material>
-  <material name="omni/Green">
-    <color rgba="0 1 0 1"/>
-  </material>
-  <material name="omni/Yellow">
-    <color rgba="1 1 0 1"/>
-  </material>
-  <material name="omni/LightGrey">
-    <color rgba="0.6 0.6 0.6 1"/>
-  </material>
-  <material name="omni/DarkGrey">
-    <color rgba="0.4 0.4 0.4 1"/>
-  </material>
-  <!-- Now we can start using the macros xacro:included above to define the actual omnipointer -->
-  <!-- The first use of a macro.  This one was defined in youbot_base/base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
-       that equals "base", and uses it to generate names for its component links and joints
-       (e.g., base_link).  The xacro:included origin block is also an argument to the macro.  By convention,
-       the origin block defines where the component is w.r.t its parent (in this case the parent
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <!-- foot for arm-->
-  <link name="base_link">
-    <inertial>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="10.0"/>
-      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-    </inertial>
-  </link>
-  <!-- joint between base_link and arm_0_link -->
-  <joint name="arm_joint_0" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0"/>
-    <parent link="base_link"/>
-    <child link="arm_link_0"/>
-  </joint>
-  <link name="arm_link_0">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-      <material name="omni/LightGrey"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <geometry>
-        <box size="0.1143 0.1143 0.0545"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.02725"/>
-      <mass value="0.2"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000267245666667" ixy="0" ixz="0" iyy="0.000267245666667" iyz="0" izz="0.000435483"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_1" type="revolute">
-    <parent link="arm_link_0"/>
-    <child link="arm_link_1"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-3.1415926535" upper="3.1415926535" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.0545"/>
-    <axis xyz="0 0 1"/>
-  </joint>
-  <link name="arm_link_1">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <geometry>
-        <box size="0.0402 0.05 0.15"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.075"/>
-      <mass value="0.190421352"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000279744834534" ixy="0" ixz="0" iyy="0.000265717763008" iyz="0" izz="6.53151584738e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_2" type="revolute">
-    <parent link="arm_link_1"/>
-    <child link="arm_link_2"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.15"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_2">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-      <material name="omni/Red"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <geometry>
-        <box size="0.0356 0.05 0.471"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.2355"/>
-      <mass value="0.29302326"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00251484771035" ixy="0" ixz="0" iyy="0.00248474836108" iyz="0" izz="9.19936757328e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_3" type="revolute">
-    <parent link="arm_link_2"/>
-    <child link="arm_link_3"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.471"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_3">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-      <material name="omni/Yellow"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <geometry>
-        <box size="0.0356 0.05 0.377"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.1885"/>
-      <mass value="0.21931466"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.000791433503053" ixy="0" ixz="0" iyy="0.000768905501178" iyz="0" izz="6.88531064581e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_4" type="revolute">
-    <parent link="arm_link_3"/>
-    <child link="arm_link_4"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.377"/>
-    <axis xyz="0 1 0"/>
-  </joint>
-  <link name="arm_link_4">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-      <material name="omni/Green"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <geometry>
-        <box size="0.0356 0.05 0.132"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.066"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_5" type="revolute">
-    <parent link="arm_link_4"/>
-    <child link="arm_link_5"/>
-    <dynamics damping="3.0" friction="0.3"/>
-    <limit effort="30.0" lower="-1.75079632679" upper="1.75079632679" velocity="5.0"/>
-    <origin rpy="0 0 0" xyz="0 0 0.132"/>
-    <axis xyz="1 0 0"/>
-  </joint>
-  <link name="arm_link_5">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-      <material name="omni/Blue"/>
-    </visual>
-    <collision>
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <geometry>
-        <box size="0.0356 0.05 0.248"/>
-      </geometry>
-    </collision>
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0.124"/>
-      <mass value="0.15813986"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="0.00037242266488" ixy="0" ixz="0" iyy="0.000356178538461" iyz="0" izz="4.96474819141e-05"/>
-    </inertial>
-  </link>
-  <joint name="arm_joint_6" type="fixed">
-    <parent link="arm_link_5"/>
-    <child link="arm_link_6"/>
-    <origin rpy="0 0 0" xyz="0 0 0.248"/>
-  </joint>
-  <link name="arm_link_6">
-    <inertial>
-      <!-- CENTER OF MASS -->
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <mass value="1e-12"/>
-      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
-      <inertia ixx="1e-12" ixy="0" ixz="0" iyy="1e-12" iyz="0" izz="1e-12"/>
-    </inertial>
-  </link>
-  <!-- END OF ARM LINKS/JOINTS -->
-</robot>
--- a/src/arm_pkg/urdf/arm12.urdf
+++ b/src/arm_pkg/urdf/arm12.urdf
@@ -1,307 +0,0 @@
-<robot name="robot">
-  <link name="base_footprint"></link>
-  <joint name="base_joint" type="fixed">
-    <parent link="base_footprint" />
-    <child link="base_link" />
-    <origin xyz="0 0 0.0004048057655643422" rpy="0 0 0" />
-  </joint>
-  <link name="base_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-      <material name="base_link-material">
-        <color rgba="0 0.6038273388475408 1 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.5 0.5 0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.17" ixy="0" ixz="0" iyy="0.17" iyz="0" izz="0.05" />
-    </inertial>
-  </link>
-  <joint name="Axis_0_Joint" type="revolute">
-    <parent link="base_link" />
-    <child link="Axis_0" />
-    <origin xyz="0 0 0.035" rpy="0 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.5" upper="2.5" velocity="0.5"/>  </joint>
-  <link name="Axis_0">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-      <material name="Axis_0-material">
-        <color rgba="0.3515325994898463 0.4735314961384573 0.9301108583738498 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.15" length="0.059" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_Joint" type="revolute">
-    <parent link="Axis_0" />
-    <child link="Axis_1" />
-    <origin xyz="0 0 0.11189588647115647" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.7" upper="1.7" velocity="0.5"/>  </joint>
-  <link name="Axis_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-      <material name="Axis_1-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.082" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_1_to_Segment_1" type="fixed">
-    <parent link="Axis_1" />
-    <child link="Segment_1" />
-    <origin xyz="0 0.2350831500270899 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_1">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-      <material name="Segment_1-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.473" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_Joint" type="revolute">
-    <parent link="Segment_1" />
-    <child link="Axis_2" />
-    <origin xyz="0 -5.219894517229704e-17 0.2637568842473722" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-2.7" upper="2.7" velocity="0.5"/>  </joint>
-  <link name="Axis_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-      <material name="Axis_2-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.055" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_2_to_Segment_2" type="fixed">
-    <parent link="Axis_2" />
-    <child link="Segment_2" />
-    <origin xyz="0 0.19535682173790003 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_2">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-      <material name="Segment_2-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.393" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_Joint" type="revolute">
-    <parent link="Segment_2" />
-    <child link="Axis_3" />
-    <origin xyz="0 -4.337792830220178e-17 0.199625776257357" rpy="1.5707963267948963 0 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-      <material name="Axis_3-material">
-        <color rgba="0.14702726648767014 0.14126329113044458 0.7304607400847158 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.05" length="0.1" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_3_to_Segment_3" type="fixed">
-    <parent link="Axis_3" />
-    <child link="Segment_3" />
-    <origin xyz="0 0.06725724726912972 0" rpy="-1.5707963267948963 0 0" />
-  </joint>
-  <link name="Segment_3">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-      <material name="Segment_3-material">
-        <color rgba="0.09084171117479915 0.3231432091022285 0.1844749944900301 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.025" length="0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Wrist_Joint" type="revolute">
-    <parent link="Segment_3" />
-    <child link="Axis_4" />
-    <origin xyz="0 0 0.0655808825338593" rpy="0 1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-1.6" upper="1.6" velocity="0.5"/>  </joint>
-  <link name="Axis_4">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-      <material name="Axis_4-material">
-        <color rgba="0.23455058215026167 0.9301108583738498 0.21952619971859377 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.03" length="0.075" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Continuous_Joint" type="revolute">
-    <parent link="Axis_4" />
-    <child link="Axis_4_C" />
-    <origin xyz="0.0009533507860803557 0 0" rpy="0 -1.5707963267948966 0" />
-    <axis xyz="0 0 1"/>
-    <limit effort="1000.0" lower="-3.14" upper="3.14" velocity="0.5"/>
-  </joint>
-  <link name="Axis_4_C">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-      <material name="Axis_4_C-material">
-        <color rgba="0.006048833020386069 0.407240211891531 0.15592646369776456 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.02" length="0.01" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.3333333333333333" ixy="0" ixz="0" iyy="0.5" iyz="0" izz="0.3333333333333333" />
-    </inertial>
-  </link>
-  <joint name="Axis_4_C_to_Effector" type="fixed">
-    <parent link="Axis_4_C" />
-    <child link="Effector" />
-    <origin xyz="0 0 0.06478774571448076" rpy="0 0 0" />
-  </joint>
-  <link name="Effector">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.05 0.01 0.135" />
-      </geometry>
-      <material name="Effector-material">
-        <color rgba="0.2746773120495699 0.01680737574872402 0.5711248294565854 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.01 0.01 0.135" />
-      </geometry>
-    </collision>
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="1" />
-      <inertia ixx="0.16666666666666666" ixy="0" ixz="0" iyy="0.16666666666666666" iyz="0" izz="0.16666666666666666" />
-    </inertial>
-  </link>
-</robot>
--- a/src/servo_arm_twist_pkg/CMakeLists.txt
+++ b/src/servo_arm_twist_pkg/CMakeLists.txt
@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.22)
+project(servo_arm_twist_pkg)
+
+# C++ Libraries #################################################
+
+# Core C++ library for calculations and collision checking.
+# Provides interface used by the component node.
+set(SERVO_LIB_NAME servo_arm_twist_lib)
+
+# Pose Tracking
+set(POSE_TRACKING pose_tracking)
+
+# Component Nodes (Shared libraries) ############################
+set(SERVO_COMPONENT_NODE servo_node)
+set(SERVO_CONTROLLER_INPUT servo_controller_input)
+
+# Executable Nodes ##############################################
+set(SERVO_NODE_MAIN_NAME servo_node_main)
+set(POSE_TRACKING_DEMO_NAME servo_pose_tracking_demo)
+set(FAKE_SERVO_CMDS_NAME fake_command_publisher)
+
+#################################################################
+
+# Common cmake code applied to all moveit packages
+find_package(moveit_common REQUIRED)
+moveit_package()
+
+set(THIS_PACKAGE_INCLUDE_DEPENDS
+  control_msgs
+  control_toolbox
+  geometry_msgs
+  moveit_core
+  moveit_msgs
+  moveit_ros_planning
+  pluginlib
+  rclcpp
+  rclcpp_components
+  sensor_msgs
+  std_msgs
+  std_srvs
+  tf2_eigen
+  trajectory_msgs
+)
+
+find_package(ament_cmake REQUIRED)
+find_package(eigen3_cmake_module REQUIRED)
+find_package(Eigen3 REQUIRED)
+foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+#####################
+## Component Nodes ##
+#####################
+
+# Add executable for using a controller
+add_library(${SERVO_CONTROLLER_INPUT} SHARED src/joystick_twist.cpp)
+ament_target_dependencies(${SERVO_CONTROLLER_INPUT} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+rclcpp_components_register_nodes(${SERVO_CONTROLLER_INPUT} "servo_arm_twist_pkg::JoyToServoPub")
+
+#############
+## Install ##
+#############
+
+# Install Libraries
+install(
+  TARGETS
+    ${SERVO_CONTROLLER_INPUT}
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+  INCLUDES DESTINATION include
+)
+
+# Install Binaries
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
+
+ament_package()
--- a/src/servo_arm_twist_pkg/README.md
+++ b/src/servo_arm_twist_pkg/README.md
@@ -0,0 +1,3 @@
+# Moveit Servo
+
+See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot.
--- a/src/servo_arm_twist_pkg/package.xml
+++ b/src/servo_arm_twist_pkg/package.xml
@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>servo_arm_twist_pkg</name>
+  <version>2.5.9</version>
+  <description>Provides real-time manipulator Cartesian and joint servoing.</description>
+  <maintainer email="blakeanderson@utexas.edu">Blake Anderson</maintainer>
+  <maintainer email="andyz@utexas.edu">Andy Zelenak</maintainer>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
+  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+
+  <license>BSD 3-Clause</license>
+
+  <url type="website">https://ros-planning.github.io/moveit_tutorials</url>
+
+  <author>Brian O'Neil</author>
+  <author email="andyz@utexas.edu">Andy Zelenak</author>
+  <author>Blake Anderson</author>
+  <author email="alex@machinekoder.com">Alexander Rössler</author>
+  <author email="tyler@picknik.ai">Tyler Weaver</author>
+  <author email="adam.pettinger@utexas.edu">Adam Pettinger</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>moveit_common</depend>
+
+  <depend>control_msgs</depend>
+  <depend>control_toolbox</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_msgs</depend>
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>tf2_eigen</depend>
+  <depend>trajectory_msgs</depend>
+
+  <exec_depend>gripper_controllers</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>joint_trajectory_controller</exec_depend>
+  <exec_depend>joy</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>tf2_ros</exec_depend>
+  <exec_depend>moveit_configs_utils</exec_depend>
+  <exec_depend>launch_param_builder</exec_depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>controller_manager</test_depend>
+  <test_depend>ros_testing</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+</package>
--- a/src/servo_arm_twist_pkg/src/joystick_twist.cpp
+++ b/src/servo_arm_twist_pkg/src/joystick_twist.cpp
@@ -0,0 +1,271 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/*      Title     : joystick_servo_example.cpp
+ *      Project   : servo_arm_twist_pkg
+ *      Created   : 08/07/2020
+ *      Author    : Adam Pettinger
+ */
+
+#include <sensor_msgs/msg/joy.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <control_msgs/msg/joint_jog.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
+#include <rclcpp/client.hpp>
+#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+#include <rclcpp/qos.hpp>
+#include <rclcpp/qos_event.hpp>
+#include <rclcpp/subscription.hpp>
+#include <rclcpp/time.hpp>
+#include <rclcpp/utilities.hpp>
+#include <thread>
+
+// We'll just set up parameters here
+const std::string JOY_TOPIC = "/joy";
+const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds";
+const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds";
+const std::string EEF_FRAME_ID = "End_Effector";
+const std::string BASE_FRAME_ID = "base_link";
+
+// Enums for button names -> axis/button array index
+// For XBOX 1 controller
+enum Axis
+{
+    LEFT_STICK_X = 0,
+    LEFT_STICK_Y = 1,
+    LEFT_TRIGGER = 2,
+    RIGHT_STICK_X = 3,
+    RIGHT_STICK_Y = 4,
+    RIGHT_TRIGGER = 5,
+    D_PAD_X = 6,
+    D_PAD_Y = 7
+};
+enum Button
+{
+    A = 0,
+    B = 1,
+    X = 2,
+    Y = 3,
+    LEFT_BUMPER = 4,
+    RIGHT_BUMPER = 5,
+    CHANGE_VIEW = 6,
+    MENU = 7,
+    HOME = 8,
+    LEFT_STICK_CLICK = 9,
+    RIGHT_STICK_CLICK = 10
+};
+
+// Some axes have offsets (e.g. the default trigger position is 1.0 not 0)
+// This will map the default values for the axes
+std::map<Axis, double> AXIS_DEFAULTS = { { LEFT_TRIGGER, 1.0 }, { RIGHT_TRIGGER, 1.0 } };
+std::map<Button, double> BUTTON_DEFAULTS;
+
+// To change controls or setup a new controller, all you should to do is change the above enums and the follow 2
+// functions
+/** \brief // This converts a joystick axes and buttons array to a TwistStamped or JointJog message
+ * @param axes The vector of continuous controller joystick axes
+ * @param buttons The vector of discrete controller button values
+ * @param twist A TwistStamped message to update in prep for publishing
+ * @param joint A JointJog message to update in prep for publishing
+ * @return return true if you want to publish a Twist, false if you want to publish a JointJog
+ */
+bool convertJoyToCmd(const std::vector<float>& axes, const std::vector<int>& buttons,
+                     std::unique_ptr<geometry_msgs::msg::TwistStamped>& twist,
+                     std::unique_ptr<control_msgs::msg::JointJog>& joint)
+{
+    // // Give joint jogging priority because it is only buttons
+    // // If any joint jog command is requested, we are only publishing joint commands
+    // if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y])
+    // {
+    //   // Map the D_PAD to the proximal joints
+    //   joint->joint_names.push_back("panda_joint1");
+    //   joint->velocities.push_back(axes[D_PAD_X]);
+    //   joint->joint_names.push_back("panda_joint2");
+    //   joint->velocities.push_back(axes[D_PAD_Y]);
+
+    //   // Map the diamond to the distal joints
+    //   joint->joint_names.push_back("panda_joint7");
+    //   joint->velocities.push_back(buttons[B] - buttons[X]);
+    //   joint->joint_names.push_back("panda_joint6");
+    //   joint->velocities.push_back(buttons[Y] - buttons[A]);
+    //   return false;
+    // }
+
+    // The bread and butter: map buttons to twist commands
+    twist->twist.linear.z = axes[RIGHT_STICK_Y];
+    twist->twist.linear.y = axes[RIGHT_STICK_X];
+
+    double lin_x_right = -0.5 * (axes[RIGHT_TRIGGER] - AXIS_DEFAULTS.at(RIGHT_TRIGGER));
+    double lin_x_left = 0.5 * (axes[LEFT_TRIGGER] - AXIS_DEFAULTS.at(LEFT_TRIGGER));
+    twist->twist.linear.x = lin_x_right + lin_x_left;
+
+    twist->twist.angular.y = axes[LEFT_STICK_Y];
+    twist->twist.angular.x = axes[LEFT_STICK_X];
+
+    double roll_positive = buttons[RIGHT_BUMPER];
+    double roll_negative = -1 * (buttons[LEFT_BUMPER]);
+    twist->twist.angular.z = roll_positive + roll_negative;
+
+    return true;
+}
+
+/** \brief // This should update the frame_to_publish_ as needed for changing command frame via controller
+ * @param frame_name Set the command frame to this
+ * @param buttons The vector of discrete controller button values
+ */
+void updateCmdFrame(std::string& frame_name, const std::vector<int>& buttons)
+{
+    if (buttons[CHANGE_VIEW] && frame_name == EEF_FRAME_ID)
+        frame_name = BASE_FRAME_ID;
+    else if (buttons[MENU] && frame_name == BASE_FRAME_ID)
+        frame_name = EEF_FRAME_ID;
+}
+
+namespace servo_arm_twist_pkg
+{
+class JoyToServoPub : public rclcpp::Node
+{
+public:
+    JoyToServoPub(const rclcpp::NodeOptions& options)
+        : Node("joy_to_twist_publisher", options), frame_to_publish_(BASE_FRAME_ID)
+    {
+        // Setup pub/sub
+        joy_sub_ = this->create_subscription<sensor_msgs::msg::Joy>(
+            JOY_TOPIC, rclcpp::SystemDefaultsQoS(),
+            [this](const sensor_msgs::msg::Joy::ConstSharedPtr& msg) { return joyCB(msg); });
+
+        twist_pub_ = this->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, rclcpp::SystemDefaultsQoS());
+        joint_pub_ = this->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, rclcpp::SystemDefaultsQoS());
+        // collision_pub_ =
+        //     this->create_publisher<moveit_msgs::msg::PlanningScene>("/planning_scene", rclcpp::SystemDefaultsQoS());
+
+        // Create a service client to start the ServoNode
+        servo_start_client_ = this->create_client<std_srvs::srv::Trigger>("/servo_node/start_servo");
+        servo_start_client_->wait_for_service(std::chrono::seconds(1));
+        servo_start_client_->async_send_request(std::make_shared<std_srvs::srv::Trigger::Request>());
+
+        // // Load the collision scene asynchronously
+        // collision_pub_thread_ = std::thread([this]() {
+        //   rclcpp::sleep_for(std::chrono::seconds(3));
+        //   // Create collision object, in the way of servoing
+        //   moveit_msgs::msg::CollisionObject collision_object;
+        //   collision_object.header.frame_id = "panda_link0";
+        //   collision_object.id = "box";
+
+        //   shape_msgs::msg::SolidPrimitive table_1;
+        //   table_1.type = table_1.BOX;
+        //   table_1.dimensions = { 0.4, 0.6, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_1_pose;
+        //   table_1_pose.position.x = 0.6;
+        //   table_1_pose.position.y = 0.0;
+        //   table_1_pose.position.z = 0.4;
+
+        //   shape_msgs::msg::SolidPrimitive table_2;
+        //   table_2.type = table_2.BOX;
+        //   table_2.dimensions = { 0.6, 0.4, 0.03 };
+
+        //   geometry_msgs::msg::Pose table_2_pose;
+        //   table_2_pose.position.x = 0.0;
+        //   table_2_pose.position.y = 0.5;
+        //   table_2_pose.position.z = 0.25;
+
+        //   collision_object.primitives.push_back(table_1);
+        //   collision_object.primitive_poses.push_back(table_1_pose);
+        //   collision_object.primitives.push_back(table_2);
+        //   collision_object.primitive_poses.push_back(table_2_pose);
+        //   collision_object.operation = collision_object.ADD;
+
+        //   moveit_msgs::msg::PlanningSceneWorld psw;
+        //   psw.collision_objects.push_back(collision_object);
+
+        //   auto ps = std::make_unique<moveit_msgs::msg::PlanningScene>();
+        //   ps->world = psw;
+        //   ps->is_diff = true;
+        //   collision_pub_->publish(std::move(ps));
+        // });
+    }
+
+    // ~JoyToServoPub() override
+    // {
+    //     if (collision_pub_thread_.joinable())
+    //         collision_pub_thread_.join();
+    // }
+
+    void joyCB(const sensor_msgs::msg::Joy::ConstSharedPtr& msg)
+    {
+        // Create the messages we might publish
+        auto twist_msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
+        auto joint_msg = std::make_unique<control_msgs::msg::JointJog>();
+
+        // This call updates the frame for twist commands
+        updateCmdFrame(frame_to_publish_, msg->buttons);
+
+        // Convert the joystick message to Twist or JointJog and publish
+        if (convertJoyToCmd(msg->axes, msg->buttons, twist_msg, joint_msg))
+        {
+            // publish the TwistStamped
+            twist_msg->header.frame_id = frame_to_publish_;
+            twist_msg->header.stamp = this->now();
+            twist_pub_->publish(std::move(twist_msg));
+        }
+        // else
+        // {
+        //     // publish the JointJog
+        //     joint_msg->header.stamp = this->now();
+        //     joint_msg->header.frame_id = "panda_link3";
+        //     joint_pub_->publish(std::move(joint_msg));
+        // }
+    }
+
+private:
+    rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+    rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
+    rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr collision_pub_;
+    rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr servo_start_client_;
+
+    std::string frame_to_publish_;
+
+    // std::thread collision_pub_thread_;
+};  // class JoyToServoPub
+
+}  // namespace servo_arm_twist_pkg
+
+// Register the component with class_loader
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(servo_arm_twist_pkg::JoyToServoPub)