From 1bec46e3e2849308233dffde8b5c3be9777447a4 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 15 May 2025 17:37:10 -0500 Subject: [PATCH 1/6] add feedback in from existing code --- src/arm_pkg/arm_pkg/arm_node.py | 90 +++++++++++++++++++++++++++++++-- 1 file changed, 87 insertions(+), 3 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 758204a..2c6a78c 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -28,6 +28,8 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.feedback_timer = self.create_timer(1.0, self.publish_feedback) + # Create subscribers 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) @@ -37,6 +39,7 @@ class SerialRelay(Node): 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.arm_feedback = SocketFeedback() # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'arm': @@ -169,9 +172,90 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - pass - #self.get_logger().info(f"[Arm Anchor] {msg.data}") - #self.send_cmd(msg.data) + output = msg.data + if output.startswith("can_relay_fromvic,arm,55"): + #pass + self.updateAngleFeedback(output) + elif output.startswith("can_relay_fromvic,arm,54"): + #pass + self.updateBusVoltage(output) + elif output.startswith("can_relay_fromvic,arm,53"): + #pass + self.updateMotorFeedback(output) + else: + return + + def publish_feedback(self): + self.arm_pub.publish(self.arm_feedback) + + def updateAngleFeedback(self, msg): + # Angle feedbacks, + #split the msg.data by commas + parts = msg.split(",") + + if len(parts) >= 7: + # Extract the angles from the string + angles_in = parts[3:7] + # Convert the angles to floats divide by 10.0 + angles = [float(angle) / 10.0 for angle in angles_in] + angles[0] = 0.0 #override axis0 to zero + # + # + #THIS NEEDS TO BE REMOVED LATER + #PLACEHOLDER FOR WRIST VALUE + # + # + angles.append(0.0)#placeholder for wrist_continuous + angles.append(0.0)#placeholder for wrist + # + # + # # 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.get_logger().info(f"Angles: {angles}") + # #debug publish angles + # tempMsg = String() + # tempMsg.data = "Angles: " + str(angles) + # #self.debug_pub.publish(tempMsg) + else: + self.get_logger().info("Invalid angle feedback input format") + + + def updateBusVoltage(self, msg): + # Bus Voltage feedbacks + parts = msg.split(",") + if len(parts) >= 7: + # 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.voltage_12 = float(voltages_in[1]) / 100.0 + self.arm_feedback.voltage_5 = float(voltages_in[2]) / 100.0 + self.arm_feedback.voltage_3 = float(voltages_in[3]) / 100.0 + else: + self.get_logger().info("Invalid voltage feedback input format") + + + def updateMotorFeedback(self, msg): + # Motor voltage/current/temperature feedback + return + # parts = msg.split(",") + # if len(parts) >= 7: + # # Extract the voltage/current/temperature from the string + # values_in = parts[3:7] + # # Convert the voltages to floats + # for i in range(4): + # #update arm_feedback's axisX_temp for each axis0_temp, axis1_temp, etc... + # pass + + # # self.arm_feedback.updateJointVoltages(i, float(values_in[i]) / 10.0) + # # self.arm_feedback.updateJointCurrents(i, float(values_in[i]) / 10.0) + # # self.arm_feedback.updateJointTemperatures(i, float(values_in[i]) / 10.0) + # else: + # self.get_logger().info("Invalid motor feedback input format") @staticmethod From 26212a837296b7c64682bc1b7b687a1d4f571088 Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 15 May 2025 19:40:41 -0500 Subject: [PATCH 2/6] arm node reference corrections --- src/arm_pkg/arm_pkg/arm_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 2c6a78c..3f1c17b 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -186,7 +186,7 @@ class SerialRelay(Node): return def publish_feedback(self): - self.arm_pub.publish(self.arm_feedback) + self.socket_pub.publish(self.arm_feedback) def updateAngleFeedback(self, msg): # Angle feedbacks, @@ -210,7 +210,7 @@ class SerialRelay(Node): # # # # 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.axis1_angle = angles[1] self.arm_feedback.axis2_angle = angles[2] From 22ccbb481fabb7117b0052a5390eeeeb4b2423bd Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Thu, 15 May 2025 20:01:25 -0500 Subject: [PATCH 3/6] Disable anchor outputting all MCU commands to screen --- src/anchor_pkg/anchor_pkg/anchor_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index f2a6108..5267ccb 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -85,7 +85,7 @@ class SerialRelay(Node): if output: # All output over debug temporarily - self.get_logger().info(f"[MCU] {output}") + #self.get_logger().info(f"[MCU] {output}") msg = String() msg.data = output if output.startswith("can_relay_fromvic,core"): From ded9d5ac788488d70be34657e69dbf4eeb31945d Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Fri, 16 May 2025 11:06:28 -0500 Subject: [PATCH 4/6] invert axis1 manual control direction --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 3f1c17b..5816c06 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -125,7 +125,7 @@ class SerialRelay(Node): def send_manual(self, msg): axis0 = msg.axis0 - axis1 = msg.axis1 + axis1 = -1 * msg.axis1 axis2 = msg.axis2 axis3 = msg.axis3 From b8c82a8e79d9eb470d830fad30ac96c2c0373f2e Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Fri, 16 May 2025 11:48:11 -0500 Subject: [PATCH 5/6] Anchor: Shutdown all nodes when one exits --- rover_launch.py | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/rover_launch.py b/rover_launch.py index 05a9599..90b4485 100644 --- a/rover_launch.py +++ b/rover_launch.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node @@ -24,7 +24,8 @@ def launch_setup(context, *args, **kwargs): executable='arm', # change as needed name='arm', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) nodes.append( @@ -33,7 +34,8 @@ def launch_setup(context, *args, **kwargs): executable='core', # change as needed name='core', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) nodes.append( @@ -42,7 +44,8 @@ def launch_setup(context, *args, **kwargs): executable='bio', # change as needed name='bio', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) nodes.append( @@ -51,7 +54,8 @@ def launch_setup(context, *args, **kwargs): executable='anchor', # change as needed name='anchor', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) elif mode in ['arm', 'core', 'bio']: @@ -63,7 +67,8 @@ def launch_setup(context, *args, **kwargs): executable='arm', name='arm', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) elif mode == 'core': @@ -73,7 +78,8 @@ def launch_setup(context, *args, **kwargs): executable='core', name='core', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) elif mode == 'bio': @@ -83,7 +89,8 @@ def launch_setup(context, *args, **kwargs): executable='bio', name='bio', output='both', - parameters=[{'launch_mode': mode}] + parameters=[{'launch_mode': mode}], + on_exit=Shutdown() ) ) else: From f70d56faf9543f20e29c7a8b76e1e2c56a5a418d Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Fri, 16 May 2025 14:18:44 -0500 Subject: [PATCH 6/6] undo override for axis0 encoder feedback --- src/arm_pkg/arm_pkg/arm_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 5816c06..c736685 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -198,7 +198,7 @@ class SerialRelay(Node): angles_in = parts[3:7] # Convert the angles to floats divide by 10.0 angles = [float(angle) / 10.0 for angle in angles_in] - angles[0] = 0.0 #override axis0 to zero + #angles[0] = 0.0 #override axis0 to zero # # #THIS NEEDS TO BE REMOVED LATER