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