diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index d394e8a..10c7fdc 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -35,12 +35,12 @@ viccan_socket_msg_len_dict = { 54: 4, 55: 4, 58: 4, - 59: 4 + 59: 4, } viccan_digit_msg_len_dict = { 54: 4, 55: 2, - 59: 2 + 59: 2, } @@ -80,10 +80,16 @@ class SerialRelay(Node): # Feedback - self.arm_feedback_pub_ = self.create_publisher(ArmFeedback, "/arm/feedback/new_feedback", qos_profile=qos.qos_profile_sensor_data) + self.arm_feedback_pub_ = self.create_publisher( + ArmFeedback, + "/arm/feedback/new_feedback", + qos_profile=qos.qos_profile_sensor_data, + ) self.arm_feedback_new = ArmFeedback() # IK: /joint_states is published from here to topic_based_control - self.joint_state_pub_ = self.create_publisher(JointState, "joint_states", qos_profile=qos.qos_profile_sensor_data) + self.joint_state_pub_ = self.create_publisher( + JointState, "joint_states", qos_profile=qos.qos_profile_sensor_data + ) self.saved_joint_state = JointState() self.saved_joint_state.name = [ "Axis_0_Joint", @@ -202,13 +208,15 @@ class SerialRelay(Node): pass def joint_command_callback(self, msg: JointState): - if len(msg.position) < 7 and len(msg.velocity) < 7: # one or the other needs to be filled - return + if len(msg.position) < 7 and len(msg.velocity) < 7: + return # command needs either position or velocity for all 7 joints # Assumed order: Axis0, Axis1, Axis2, Axis3, Wrist_Yaw, Wrist_Roll, Gripper # TODO: formalize joint names in URDF, refactor here to depend on joint names # Embedded takes deg*10, ROS2 uses Radians - velocities = [math.degrees(vel) * 10 if abs(vel) > 0.05 else 0.0 for vel in msg.velocity] + velocities = [ + math.degrees(vel) * 10 if abs(vel) > 0.05 else 0.0 for vel in msg.velocity + ] # Axis 2 & 3 URDF direction is inverted velocities[2] = -velocities[2] velocities[3] = -velocities[3] @@ -289,7 +297,7 @@ class SerialRelay(Node): self.process_fromvic_arm(msg) elif msg.mcu_name == "digit": self.process_fromvic_digit(msg) - + def process_fromvic_arm(self, msg: VicCAN): if msg.mcu_name != "arm": return @@ -322,7 +330,7 @@ class SerialRelay(Node): motor.voltage = float(msg.data[2]) / 10.0 motor.current = float(msg.data[3]) / 10.0 motor.header.stamp = msg.header.stamp - + self.arm_feedback_pub_.publish(self.arm_feedback_new) case 54: # Board voltages self.arm_feedback_new.socket_voltage.vbatt = float(msg.data[0]) / 100.0 @@ -334,8 +342,12 @@ class SerialRelay(Node): # Joint state publisher for URDF visualization self.saved_joint_state.position[0] = math.radians(angles[0]) # Axis 0 self.saved_joint_state.position[1] = math.radians(angles[1]) # Axis 1 - self.saved_joint_state.position[2] = math.radians(-angles[2]) # Axis 2 (inverted) - self.saved_joint_state.position[3] = math.radians(-angles[3]) # Axis 3 (inverted) + self.saved_joint_state.position[2] = math.radians( + -angles[2] + ) # Axis 2 (inverted) + self.saved_joint_state.position[3] = math.radians( + -angles[3] + ) # Axis 3 (inverted) # Wrist is handled by digit feedback self.saved_joint_state.header.stamp = msg.header.stamp self.joint_state_pub_.publish(self.saved_joint_state) @@ -351,7 +363,7 @@ class SerialRelay(Node): motor = self.arm_feedback_new.axis3_motor case 4: motor = self.arm_feedback_new.axis0_motor - + if motor: motor.position = float(msg.data[1]) motor.velocity = float(msg.data[2]) @@ -360,10 +372,18 @@ class SerialRelay(Node): self.arm_feedback_pub_.publish(self.arm_feedback_new) case 59: # Arm joint velocities velocities = [vel / 100.0 for vel in msg.data] # VicCAN sends deg/s*100 - self.saved_joint_state.velocity[0] = math.radians(velocities[0]) # Axis 0 - self.saved_joint_state.velocity[1] = math.radians(velocities[1]) # Axis 1 - self.saved_joint_state.velocity[2] = math.radians(-velocities[2]) # Axis 2 (-) - self.saved_joint_state.velocity[3] = math.radians(-velocities[3]) # Axis 3 (-) + self.saved_joint_state.velocity[0] = math.radians( + velocities[0] + ) # Axis 0 + self.saved_joint_state.velocity[1] = math.radians( + velocities[1] + ) # Axis 1 + self.saved_joint_state.velocity[2] = math.radians( + -velocities[2] + ) # Axis 2 (-) + self.saved_joint_state.velocity[3] = math.radians( + -velocities[3] + ) # Axis 3 (-) # Wrist is handled by digit feedback self.saved_joint_state.header.stamp = msg.header.stamp self.joint_state_pub_.publish(self.saved_joint_state) @@ -387,8 +407,12 @@ class SerialRelay(Node): self.arm_feedback_new.digit_voltage.v12 = float(msg.data[1]) / 100.0 self.arm_feedback_new.digit_voltage.v5 = float(msg.data[2]) / 100.0 case 55: # Arm joint positions - self.saved_joint_state.position[4] = math.radians(msg.data[0]) # Wrist roll - self.saved_joint_state.position[5] = math.radians(msg.data[1]) # Wrist yaw + self.saved_joint_state.position[4] = math.radians( + msg.data[0] + ) # Wrist roll + self.saved_joint_state.position[5] = math.radians( + msg.data[1] + ) # Wrist yaw def publish_feedback(self): self.socket_pub.publish(self.arm_feedback)