From ff68d53e24672cf025df292f347134430ae70f14 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 05:46:19 +0000 Subject: [PATCH 01/12] feat: update for new faerie arm --- src/anchor_pkg/anchor_pkg/anchor_node.py | 16 ++++++++-------- src/bio_pkg/bio_pkg/bio_node.py | 21 +++++++++++---------- src/ros2_interfaces_pkg | 2 +- 3 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 5267ccb..071a71a 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -90,9 +90,9 @@ class SerialRelay(Node): msg.data = output if output.startswith("can_relay_fromvic,core"): self.core_pub.publish(msg) - elif output.startswith("can_relay_fromvic,arm"): + elif output.startswith("can_relay_fromvic,arm") or output.startswith("can_relay_fromvic,digit"): # digit for voltage readings self.arm_pub.publish(msg) - elif output.startswith("can_relay_fromvic,bio"): + elif output.startswith("can_relay_fromvic,citadel") or output.startswith("can_relay_fromvic,digit"): # digit for SHT sensor self.bio_pub.publish(msg) # msg = String() # msg.data = output @@ -135,16 +135,16 @@ def myexcepthook(type, value, tb): print("Uncaught exception:", type, value) if serial_pub: serial_pub.cleanup() - + def main(args=None): - rclpy.init(args=args) - sys.excepthook = myexcepthook + rclpy.init(args=args) + sys.excepthook = myexcepthook - global serial_pub + global serial_pub - serial_pub = SerialRelay() - serial_pub.run() + serial_pub = SerialRelay() + serial_pub.run() if __name__ == '__main__': signal.signal(signal.SIGTSTP, lambda signum, frame: sys.exit(0)) # Catch Ctrl+Z and exit cleanly diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 1523ecd..f4b582f 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -9,6 +9,7 @@ import atexit import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import BioControl +from ros2_interfaces_pkg.msg import BioFeedback serial_pub = None thread = None @@ -25,10 +26,10 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) - #self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10) - # Create subscribers\ + # Create subscribers self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10) # Topics used in anchor mode @@ -136,7 +137,7 @@ class SerialRelay(Node): self.send_cmd(command) - # LSS + # LSS (SCYTHE) command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n" self.send_cmd(command) # Vibration Motor @@ -150,18 +151,18 @@ class SerialRelay(Node): # To be reviewed before use# # Laser - command = "can_relay_tovic,faerie,28," + str(msg.laser) + "\n" + command = "can_relay_tovic,digit,28," + str(msg.laser) + "\n" self.send_cmd(command) - - # # UV Light - # command = "can_relay_tovic,faerie,38," + str(msg.uvLight) + "\n" - # self.send_cmd(command) - # Drill - command = f"can_relay_tovic,faerie,19,{msg.drill_duty:.2f}\n" + # Drill (SCABBARD) + command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n" print(msg.drill_duty) self.send_cmd(command) + # Bio linear actuator + command = "can_relay_tovic,digit,42," + str(msg.drill_arm) + "\n" + self.send_cmd(command) + def send_cmd(self, msg): diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index f7f39a6..3db6c94 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit f7f39a635287f242a8feacb1792332c0521fc5a7 +Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 From fa057bb4ace60d8b9e4b4744d7a117daabf16c3d Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 06:19:20 +0000 Subject: [PATCH 02/12] feat: add digit voltage feedback --- src/arm_pkg/arm_pkg/arm_node.py | 16 +++++++++++++--- src/ros2_interfaces_pkg | 2 +- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index c736685..45ab4b2 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -10,7 +10,7 @@ import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import ArmManual from ros2_interfaces_pkg.msg import ArmIK -from ros2_interfaces_pkg.msg import SocketFeedback +from ros2_interfaces_pkg.msg import ArmFeedback serial_pub = None thread = None @@ -27,7 +27,7 @@ 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.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10) self.feedback_timer = self.create_timer(1.0, self.publish_feedback) # Create subscribers @@ -39,7 +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() + self.arm_feedback = ArmFeedback() # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'arm': @@ -182,6 +182,16 @@ class SerialRelay(Node): elif output.startswith("can_relay_fromvic,arm,53"): #pass self.updateMotorFeedback(output) + elif output.startswith("can_relay_fromvic,digit,54"): + 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.digit_bat_voltage = float(voltages_in[0]) / 100.0 + self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0 + self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0 + self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0 else: return diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 3db6c94..fac196b 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 +Subproject commit fac196b5d6efb52db9490c1326629ac2d8f63c4e From 02b8904d631d52bde9c8d8babac3e18c9a1d6ffc Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 06:37:14 +0000 Subject: [PATCH 03/12] feat: add bio feedback --- src/bio_pkg/bio_pkg/bio_node.py | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index f4b582f..bf84cde 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -26,17 +26,22 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) - self.bio_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10) + self.feedback_pub = self.create_publisher(BioFeedback, '/bio/feedback', 10) # Create subscribers self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10) + + # Create a publisher for telemetry + self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) # Topics used in anchor mode if self.launch_mode == 'anchor': self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) + self.bio_feedback = BioFeedback() + # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'bio': @@ -175,9 +180,20 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): + output = msg.data + parts = str(output.strip()).split(",") self.get_logger().info(f"[Bio Anchor] {msg.data}") - #self.send_cmd(msg.data) + if output.startswith("can_relay_fromvic,citadel,54"):#bat, 12, 5, 3, Voltage readings * 100 + self.bio_feedback.bat_voltage = float(parts[3]) / 100.0 + self.bio_feedback.voltage_12 = float(parts[4]) / 100.0 + self.bio_feedback.voltage_5 = float(parts[5]) / 100.0 + if output.startswith("can_relay_fromvic,digit,57"): + self.bio_feedback.drill_temp = float(parts[3]) + self.bio_feedback.drill_humidity = float(parts[4]) + + def publish_feedback(self): + self.feedback_pub.publish(self.bio_feedback) @staticmethod def list_serial_ports(): From 76ca9d81b227e1255099cdf4b922459c2ef75f61 Mon Sep 17 00:00:00 2001 From: David Date: Fri, 23 May 2025 22:42:12 +0000 Subject: [PATCH 04/12] feat: actually add digit feedback --- src/arm_pkg/arm_pkg/arm_node.py | 21 ++++++++++++++------- src/bio_pkg/bio_pkg/bio_node.py | 2 +- src/ros2_interfaces_pkg | 2 +- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 45ab4b2..0536590 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -10,7 +10,8 @@ import signal from std_msgs.msg import String from ros2_interfaces_pkg.msg import ArmManual from ros2_interfaces_pkg.msg import ArmIK -from ros2_interfaces_pkg.msg import ArmFeedback +from ros2_interfaces_pkg.msg import SocketFeedback +from ros2_interfaces_pkg.msg import DigitFeedback serial_pub = None thread = None @@ -27,7 +28,8 @@ class SerialRelay(Node): # Create publishers self.debug_pub = self.create_publisher(String, '/arm/feedback/debug', 10) - self.socket_pub = self.create_publisher(ArmFeedback, '/arm/feedback/socket', 10) + self.socket_pub = self.create_publisher(SocketFeedback, '/arm/feedback/socket', 10) + self.digit_pub = self.create_publisher(DigitFeedback, '/arm/feedback/digit', 10) self.feedback_timer = self.create_timer(1.0, self.publish_feedback) # Create subscribers @@ -39,7 +41,8 @@ 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 = ArmFeedback() + self.arm_feedback = SocketFeedback() + self.digit_feedback = DigitFeedback() # Search for ports IF in 'arm' (standalone) and not 'anchor' mode if self.launch_mode == 'arm': @@ -188,15 +191,19 @@ class SerialRelay(Node): # Extract the voltage from the string voltages_in = parts[3:7] # Convert the voltages to floats - self.arm_feedback.digit_bat_voltage = float(voltages_in[0]) / 100.0 - self.arm_feedback.digit_voltage_12 = float(voltages_in[1]) / 100.0 - self.arm_feedback.digit_voltage_5 = float(voltages_in[2]) / 100.0 - self.arm_feedback.digit_voltage_3 = float(voltages_in[3]) / 100.0 + self.digit_feedback.bat_voltage = float(voltages_in[0]) / 100.0 + self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 + self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 + elif output.startswith("can_relay_fromvic,digit,55"): + parts = msg.split(",") + if len(parts) >= 4: + self.digit_feedback.wrist_angle = float(parts[3]) else: return def publish_feedback(self): self.socket_pub.publish(self.arm_feedback) + self.digit_pub.publish(self.digit_feedback) def updateAngleFeedback(self, msg): # Angle feedbacks, diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index bf84cde..2f5d580 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -184,7 +184,7 @@ class SerialRelay(Node): parts = str(output.strip()).split(",") self.get_logger().info(f"[Bio Anchor] {msg.data}") - if output.startswith("can_relay_fromvic,citadel,54"):#bat, 12, 5, 3, Voltage readings * 100 + if output.startswith("can_relay_fromvic,citadel,54"): # bat, 12, 5, Voltage readings * 100 self.bio_feedback.bat_voltage = float(parts[3]) / 100.0 self.bio_feedback.voltage_12 = float(parts[4]) / 100.0 self.bio_feedback.voltage_5 = float(parts[5]) / 100.0 diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index fac196b..3db6c94 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit fac196b5d6efb52db9490c1326629ac2d8f63c4e +Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 From 6b0ea58e952173a68b4168982af7fd9da4c556f3 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 24 May 2025 00:02:28 +0000 Subject: [PATCH 05/12] feat: add imu calibration for core --- src/core_pkg/core_pkg/core_node.py | 1 + src/ros2_interfaces_pkg | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index a2b5cd3..95228ea 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -203,6 +203,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) + self.core_feedback.imu_calib = float(parts[6]) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 3db6c94..021dd0c 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 3db6c9483300ddec637c8745bcde3d1017686470 +Subproject commit 021dd0c3dd0bdbad0e653388fa6c73188257dd9b From f34ef88d6ce2ef4f46ea3871bfb40abab33c5bfe Mon Sep 17 00:00:00 2001 From: David Date: Sat, 24 May 2025 16:03:33 +0000 Subject: [PATCH 06/12] feat: add brake mode to core --- src/core_pkg/core_pkg/core_node.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 95228ea..3cc1974 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -172,12 +172,15 @@ class SerialRelay(Node): def send_controls(self, msg): #can_relay_tovic,core,19, left_stick, right_stick if(msg.turn_to_enable): - command = "can_relay_tovic,core,41," + msg.turn_to + ',' + msg.turn_to_timeout + '\n' + command = "can_relay_tovic,core,41," + str(msg.turn_to) + ',' + str(msg.turn_to_timeout) + '\n' else: command = "can_relay_tovic,core,19," + self.scale_duty(msg.left_stick, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n' - self.send_cmd(command) - + + # Brake mode + command = "can_relay_tovic,core,18," + str(int(msg.brake)) + '\n' + self.send_cmd(command) + #print(f"[Sys] Relaying: {command}") def send_cmd(self, msg): if self.launch_mode == 'anchor': From 1d0292b4c926a3f9fceb235139af0df751f70d87 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 25 May 2025 23:25:23 +0000 Subject: [PATCH 07/12] refactor: change servo_position to servo_state wiggle --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- src/ros2_interfaces_pkg | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 2f5d580..8785d3a 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -138,8 +138,8 @@ class SerialRelay(Node): self.send_cmd(command) # Servos, only send if not zero if msg.servo_id != 0: - command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(msg.servo_position) + "\n" - self.send_cmd(command) + command = "can_relay_tovic,citadel,25," + str(msg.servo_id) + "," + str(int(msg.servo_state)) + "\n" + self.send_cmd(command) # LSS (SCYTHE) diff --git a/src/ros2_interfaces_pkg b/src/ros2_interfaces_pkg index 021dd0c..aec2208 160000 --- a/src/ros2_interfaces_pkg +++ b/src/ros2_interfaces_pkg @@ -1 +1 @@ -Subproject commit 021dd0c3dd0bdbad0e653388fa6c73188257dd9b +Subproject commit aec22081e3deb6f7092bb79393ecb234193a75ab From 9f3ff0f8df28b382d287c53d30773443a569cd55 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:34:16 +0000 Subject: [PATCH 08/12] fix: interface names --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 8785d3a..3aa2691 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -123,7 +123,7 @@ class SerialRelay(Node): pass - def send_control(self, msg): + def send_control(self, msg: BioControl): # CITADEL Control Commands ################ @@ -143,7 +143,7 @@ class SerialRelay(Node): # LSS (SCYTHE) - command = "can_relay_tovic,citadel,24," + str(msg.lss_direction) + "\n" + command = "can_relay_tovic,citadel,24," + str(msg.bio_arm) + "\n" self.send_cmd(command) # Vibration Motor command = "can_relay_tovic,citadel,26," + str(msg.vibration_motor) + "\n" diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 3cc1974..ffa2d11 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -206,7 +206,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) - self.core_feedback.imu_calib = float(parts[6]) + self.core_feedback.imu_calib = int(parts[6]) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) From 7027900620d29a169446e5150cb721d560ba3ec3 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:36:51 +0000 Subject: [PATCH 09/12] fix: round float imu_calib --- src/core_pkg/core_pkg/core_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index ffa2d11..0f1052f 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -192,7 +192,7 @@ class SerialRelay(Node): self.get_logger().info(f"[Core to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) - def anchor_feedback(self, msg): + def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",") #self.get_logger().info(f"[ANCHOR FEEDBACK parts] {parts}") @@ -206,7 +206,7 @@ class SerialRelay(Node): self.core_feedback.bno_gyro.x = float(parts[3]) self.core_feedback.bno_gyro.y = float(parts[4]) self.core_feedback.bno_gyro.z = float(parts[5]) - self.core_feedback.imu_calib = int(parts[6]) + self.core_feedback.imu_calib = round(float(parts[6])) elif output.startswith("can_relay_fromvic,core,52"):#Accel x,y,z, heading *10 self.core_feedback.bno_accel.x = float(parts[3]) self.core_feedback.bno_accel.y = float(parts[4]) From 1486b87156435e9c14f53c059eda26e458049e37 Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:38:55 +0000 Subject: [PATCH 10/12] fix: drill_duty --- src/bio_pkg/bio_pkg/bio_node.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index 3aa2691..be0103d 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -160,8 +160,8 @@ class SerialRelay(Node): self.send_cmd(command) # Drill (SCABBARD) - command = f"can_relay_tovic,digit,19,{msg.drill_duty:.2f}\n" - print(msg.drill_duty) + command = f"can_relay_tovic,digit,19,{msg.drill:.2f}\n" + print(msg.drill) self.send_cmd(command) # Bio linear actuator From 39f2aa3a18bd35a65461b7345711c002e4baecff Mon Sep 17 00:00:00 2001 From: David Date: Mon, 26 May 2025 02:55:14 +0000 Subject: [PATCH 11/12] fix: msg.split to msg.data.split --- src/arm_pkg/arm_pkg/arm_node.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 0536590..ab8260f 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): #pass self.updateMotorFeedback(output) elif output.startswith("can_relay_fromvic,digit,54"): - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -195,7 +195,7 @@ class SerialRelay(Node): self.digit_feedback.voltage_12 = float(voltages_in[1]) / 100.0 self.digit_feedback.voltage_5 = float(voltages_in[2]) / 100.0 elif output.startswith("can_relay_fromvic,digit,55"): - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 4: self.digit_feedback.wrist_angle = float(parts[3]) else: @@ -205,10 +205,10 @@ class SerialRelay(Node): self.socket_pub.publish(self.arm_feedback) self.digit_pub.publish(self.digit_feedback) - def updateAngleFeedback(self, msg): + def updateAngleFeedback(self, msg: String): # Angle feedbacks, #split the msg.data by commas - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the angles from the string @@ -241,9 +241,9 @@ class SerialRelay(Node): self.get_logger().info("Invalid angle feedback input format") - def updateBusVoltage(self, msg): + def updateBusVoltage(self, msg: String): # Bus Voltage feedbacks - parts = msg.split(",") + parts = msg.data.split(",") if len(parts) >= 7: # Extract the voltage from the string voltages_in = parts[3:7] @@ -259,7 +259,7 @@ class SerialRelay(Node): def updateMotorFeedback(self, msg): # Motor voltage/current/temperature feedback return - # parts = msg.split(",") + # parts = msg.data.split(",") # if len(parts) >= 7: # # Extract the voltage/current/temperature from the string # values_in = parts[3:7] From bff729f25a5cde22d837ab86797c0f50bd7dfca3 Mon Sep 17 00:00:00 2001 From: David Date: Tue, 27 May 2025 19:59:13 +0000 Subject: [PATCH 12/12] style: add a few type annotations --- src/bio_pkg/bio_pkg/bio_node.py | 6 +++--- src/core_pkg/core_pkg/core_node.py | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/bio_pkg/bio_pkg/bio_node.py b/src/bio_pkg/bio_pkg/bio_node.py index be0103d..e901e5b 100644 --- a/src/bio_pkg/bio_pkg/bio_node.py +++ b/src/bio_pkg/bio_pkg/bio_node.py @@ -170,7 +170,7 @@ class SerialRelay(Node): - def send_cmd(self, msg): + def send_cmd(self, msg: str): if self.launch_mode == 'anchor': #if in anchor mode, send to anchor node to relay output = String() output.data = msg @@ -179,7 +179,7 @@ class SerialRelay(Node): self.get_logger().info(f"[Bio to MCU] {msg}") self.ser.write(bytes(msg, "utf8")) - def anchor_feedback(self, msg): + def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",") self.get_logger().info(f"[Bio Anchor] {msg.data}") @@ -188,7 +188,7 @@ class SerialRelay(Node): self.bio_feedback.bat_voltage = float(parts[3]) / 100.0 self.bio_feedback.voltage_12 = float(parts[4]) / 100.0 self.bio_feedback.voltage_5 = float(parts[5]) / 100.0 - if output.startswith("can_relay_fromvic,digit,57"): + elif output.startswith("can_relay_fromvic,digit,57"): self.bio_feedback.drill_temp = float(parts[3]) self.bio_feedback.drill_humidity = float(parts[4]) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 0f1052f..ca3555b 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -152,7 +152,7 @@ class SerialRelay(Node): self.ser.close() self.exit(1) - def scale_duty(self, value, max_speed): + def scale_duty(self, value: float, max_speed: float): leftMin = -1 leftMax = 1 rightMin = -max_speed/100.0