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

refactor some things, reenable feedback

Browse Source
This commit is contained in:
Tristan McGinnis
2025-05-04 13:56:14 -05:00
committed by David
parent 9d13d487cb
commit 440a94f0cc
1 changed files with 31 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -52,9 +52,11 @@ class SerialRelay(Node):
self.ik_sub = self.create_subscription(ArmIK, '/arm/control/ik', self.send_ik, 10) 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)
# Topics used in anchor mode # Topics used in anchor mode
if self.launch_mode == 'anchor': if self.launch_mode == 'anchor':
#self.anchor_sub = self.create_subscription(String, '/anchor/arm/feedback', self.anchor_feedback, 10) 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.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
@@ -130,13 +132,13 @@ class SerialRelay(Node):
if output: if output:
if output.startswith("can_relay_fromvic,arm,55"): if output.startswith("can_relay_fromvic,arm,55"):
pass pass
#self.recordAngleFeedback(output) #self.updateAngleFeedback(output)
elif output.startswith("can_relay_fromvic,arm,54"): elif output.startswith("can_relay_fromvic,arm,54"):
pass pass
#self.recordBusVoltage(output) #self.updateBusVoltage(output)
elif output.startswith("can_relay_fromvic,arm,53"): elif output.startswith("can_relay_fromvic,arm,53"):
pass pass
#self.recordMotorFeedback(output) #self.updateMotorFeedback(output)
self.get_logger().info(f"[MCU] {output}") self.get_logger().info(f"[MCU] {output}")
msg = String() msg = String()
msg.data = "From MCU Got: " + output msg.data = "From MCU Got: " + output
@@ -160,7 +162,7 @@ class SerialRelay(Node):
pass pass
def recordAngleFeedback(self, msg): def updateAngleFeedback(self, msg):
# Angle feedbacks # Angle feedbacks
parts = msg.data.split(",") parts = msg.data.split(",")
if len(parts) >= 7: if len(parts) >= 7:
@@ -181,10 +183,10 @@ class SerialRelay(Node):
# #
# # Update the arm's current angles # # Update the arm's current angles
# self.arm.update_angles(angles) # self.arm.update_angles(angles)
# self.arm_feedback.axis0_angle = angles[0] self.arm_feedback.axis0_angle = angles[0]
# self.arm_feedback.axis1_angle = angles[1] self.arm_feedback.axis1_angle = angles[1]
# self.arm_feedback.axis2_angle = angles[2] self.arm_feedback.axis2_angle = angles[2]
# self.arm_feedback.axis3_angle = angles[3] self.arm_feedback.axis3_angle = angles[3]
# self.get_logger().info(f"Angles: {angles}") # self.get_logger().info(f"Angles: {angles}")
# #debug publish angles # #debug publish angles
# tempMsg = String() # tempMsg = String()
@@ -193,7 +195,7 @@ class SerialRelay(Node):
else: else:
self.get_logger().info("Invalid angle feedback input format") self.get_logger().info("Invalid angle feedback input format")
def recordBusVoltage(self, msg): def updateBusVoltage(self, msg):
# Bus Voltage feedbacks # Bus Voltage feedbacks
parts = msg.data.split(",") parts = msg.data.split(",")
if len(parts) >= 7: if len(parts) >= 7:
@@ -207,7 +209,7 @@ class SerialRelay(Node):
else: else:
self.get_logger().info("Invalid voltage feedback input format") self.get_logger().info("Invalid voltage feedback input format")
def recordMotorFeedback(self, msg): def updateMotorFeedback(self, msg):
# Motor voltage/current/temperature feedback # Motor voltage/current/temperature feedback
parts = msg.data.split(",") parts = msg.data.split(",")
if len(parts) >= 7: if len(parts) >= 7:
@@ -230,9 +232,9 @@ class SerialRelay(Node):
axis2 = msg.axis2 axis2 = msg.axis2
axis3 = msg.axis3 axis3 = msg.axis3
tempMsg = String() # tempMsg = String()
tempMsg.data = "Sending manual" # tempMsg.data = "Sending manual"
self.debug_pub.publish(tempMsg) # self.debug_pub.publish(tempMsg)
#Send controls for arm #Send controls for arm
@@ -380,7 +382,7 @@ class SerialRelay(Node):
else: else:
self.get_logger().info("Invalid voltage feedback input format") self.get_logger().info("Invalid voltage feedback input format")
def socket_pub_callback(self): def publish_feedback(self):
# Create a SocketFeedback message and publish it # Create a SocketFeedback message and publish it
# msg = SocketFeedback() # msg = SocketFeedback()
# msg.bat_voltage = self.arm_feedback.bat_voltage # msg.bat_voltage = self.arm_feedback.bat_voltage
@@ -394,7 +396,7 @@ class SerialRelay(Node):
#debug print #debug print
self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm self.socket_pub.publish(self.arm_feedback) #Publish feedback from arm
self.arm.update_position() #Run FK and update the current position of the arm, using FK #self.arm.update_position() #Run FK and update the current position of the arm, using FK
@@ -414,12 +416,17 @@ class SerialRelay(Node):
target_position = current_position + input_raw 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)
self.ik_debug.publish(tempMsg)
# Debug output for current position # Debug output for current position
tempMsg.data = "Current Position: " + str(current_position) #tempMsg.data = "Current Position: " + str(current_position)
#self.debug_pub.publish(tempMsg) #self.debug_pub.publish(tempMsg)
# Debug output for target position # Debug output for target position
tempMsg.data = "Target Position: " + str(target_position) #tempMsg.data = "Target Position: " + str(target_position)
#self.debug_pub.publish(tempMsg) #self.debug_pub.publish(tempMsg)
# Perform IK with the target position # Perform IK with the target position
@@ -429,20 +436,19 @@ class SerialRelay(Node):
self.send_cmd(command) self.send_cmd(command)
self.get_logger().info(f"IK Success: {target_position}") self.get_logger().info(f"IK Success: {target_position}")
tempMsg = String() # tempMsg = String()
tempMsg.data = "IK Success: " + str(target_position) # tempMsg.data = "IK Success: " + str(target_position)
#self.debug_pub.publish(tempMsg) # #self.debug_pub.publish(tempMsg)
tempMsg.data = "Sending: " + str(command) # tempMsg.data = "Sending: " + str(command)
#self.debug_pub.publish(tempMsg) #self.debug_pub.publish(tempMsg)
else: else:
self.get_logger().info("IK Fail") self.get_logger().info("IK Fail")
tempMsg = String() # tempMsg = String()
tempMsg.data = "IK Fail" # tempMsg.data = "IK Fail"
#self.debug_pub.publish(tempMsg) #self.debug_pub.publish(tempMsg)
# Manual control for Wrist/Effector # Manual control for Wrist/Effector
command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n" command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
self.send_cmd(command) self.send_cmd(command)
command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n" command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"