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.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
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)
@@ -130,13 +132,13 @@ class SerialRelay(Node):
if output:
if output.startswith("can_relay_fromvic,arm,55"):
pass
#self.recordAngleFeedback(output)
#self.updateAngleFeedback(output)
elif output.startswith("can_relay_fromvic,arm,54"):
pass
#self.recordBusVoltage(output)
#self.updateBusVoltage(output)
elif output.startswith("can_relay_fromvic,arm,53"):
pass
#self.recordMotorFeedback(output)
#self.updateMotorFeedback(output)
self.get_logger().info(f"[MCU] {output}")
msg = String()
msg.data = "From MCU Got: " + output
@@ -160,7 +162,7 @@ class SerialRelay(Node):
pass
def recordAngleFeedback(self, msg):
def updateAngleFeedback(self, msg):
# Angle feedbacks
parts = msg.data.split(",")
if len(parts) >= 7:
@@ -181,10 +183,10 @@ class SerialRelay(Node):
#
# # 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.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()
@@ -193,7 +195,7 @@ class SerialRelay(Node):
else:
self.get_logger().info("Invalid angle feedback input format")
def recordBusVoltage(self, msg):
def updateBusVoltage(self, msg):
# Bus Voltage feedbacks
parts = msg.data.split(",")
if len(parts) >= 7:
@@ -207,7 +209,7 @@ class SerialRelay(Node):
else:
self.get_logger().info("Invalid voltage feedback input format")
def recordMotorFeedback(self, msg):
def updateMotorFeedback(self, msg):
# Motor voltage/current/temperature feedback
parts = msg.data.split(",")
if len(parts) >= 7:
@@ -230,9 +232,9 @@ class SerialRelay(Node):
axis2 = msg.axis2
axis3 = msg.axis3
tempMsg = String()
tempMsg.data = "Sending manual"
self.debug_pub.publish(tempMsg)
# tempMsg = String()
# tempMsg.data = "Sending manual"
# self.debug_pub.publish(tempMsg)
#Send controls for arm
@@ -380,7 +382,7 @@ class SerialRelay(Node):
else:
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
# msg = SocketFeedback()
# msg.bat_voltage = self.arm_feedback.bat_voltage
@@ -394,7 +396,7 @@ class SerialRelay(Node):
#debug print
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
#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
tempMsg.data = "Current Position: " + str(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)
#tempMsg.data = "Target Position: " + str(target_position)
#self.debug_pub.publish(tempMsg)
# Perform IK with the target position
@@ -429,20 +436,19 @@ class SerialRelay(Node):
self.send_cmd(command)
self.get_logger().info(f"IK Success: {target_position}")
tempMsg = String()
tempMsg.data = "IK Success: " + str(target_position)
#self.debug_pub.publish(tempMsg)
tempMsg.data = "Sending: " + str(command)
# tempMsg = String()
# tempMsg.data = "IK Success: " + str(target_position)
# #self.debug_pub.publish(tempMsg)
# tempMsg.data = "Sending: " + str(command)
#self.debug_pub.publish(tempMsg)
else:
self.get_logger().info("IK Fail")
tempMsg = String()
tempMsg.data = "IK Fail"
# tempMsg = String()
# tempMsg.data = "IK Fail"
#self.debug_pub.publish(tempMsg)
# Manual control for Wrist/Effector
command = "can_relay_tovic,digit,35," + str(msg.effector_roll) + "\n"
self.send_cmd(command)
command = "can_relay_tovic,digit,36,0," + str(msg.effector_yaw) + "\n"