aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

style: (arm) format with black (oops)

Browse Source
This commit is contained in:
David
2025-12-22 14:59:22 -06:00
parent 073b9373bc
commit 8f9a2d566d
1 changed files with 42 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
src/arm_pkg/arm_pkg/arm_node.py
View File

@@ -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)