Revert "add anchor relay to feedback topics for bio,arm,core"

This reverts commit d38a84abca.

src/anchor_pkg/anchor_pkg/anchor_node.py

@@ -84,21 +84,6 @@ class SerialRelay(Node):
             if output:
                 # All output over debug temporarily
                 self.get_logger().info(f"[MCU] {output}")
-                if output.startswith("can_relay_fromvic,arm"):
-                    # Publish the message to the arm topic
-                    msg = String()
-                    msg.data = output
-                    self.arm_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,core"):
-                    # Publish the message to the core topic
-                    msg = String()
-                    msg.data = output
-                    self.core_pub.publish(msg)
-                elif output.startswith("can_relay_fromvic,bio"):
-                    # Publish the message to the bio topic
-                    msg = String()
-                    msg.data = output
-                    self.bio_pub.publish(msg)
                 msg = String()
                 msg.data = output
                 self.debug_pub.publish(msg)

src/arm_pkg/arm_pkg/arm_node.py

@@ -206,10 +206,10 @@ class SerialRelay(Node):
                 # Extract the voltage from the string
                 voltages_in = parts[3:7]
                 # Convert the voltages to floats
-                # self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
+                self.arm_feedback.bat_voltage = float(voltages_in[0]) / 100.0
-                # self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
+                self.arm_feedback.voltage_12 = float(voltages_in[1]) / 100.0
-                # self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
+                self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0
-                # self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
+                self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0
             else:
                 self.get_logger().info("Invalid voltage feedback input format")
                 

src/arm_pkg/arm_pkg/astra_arm.py

@@ -1,137 +0,0 @@
-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, angles[0]*degree, angles[1]*degree, 0.0, angles[2]*degree, 0.0, angles[3]*degree, 0.0, angles[4]*degree, angles[5]*degree, 0.0]
-
-
-class Arm:
-    def __init__(self, urdf_name):
-        self.ik_tolerance = 1e-3 #Tolerance (in meters) to determine if solution is valid
-        # URDF file path
-        self.urdf = os.path.join(get_package_share_directory('arm_pkg'), 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 = []
-        self.target_position = [0.0, 0.0, 0.0] 
-        self.target_orientation = [] # 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):
-        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]
-            
-            # print(f"[TRY] FK Position for Solution: {fk_position}")
-
-            error = np.linalg.norm(target_position - fk_position)
-            if error > self.ik_tolerance:
-                print(f"No VALID IK Solution within tolerance. Error: {error}")
-                return False
-            else:
-                print(f"IK Solution Found. Error: {error}")
-                return True
-        except Exception as e:
-            print(f"IK failed for exception: {e}")
-            return False
-
-    # 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]
-
-    # 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]
-        
-        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]
-
-        # 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]
-
-
-