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

feat: add bio feedback

Browse Source
This commit is contained in:
David
2025-05-23 06:37:14 +00:00
parent fa057bb4ac
commit 02b8904d63
1 changed files with 18 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/bio_pkg/bio_pkg/bio_node.py
View File

@@ -26,17 +26,22 @@ class SerialRelay(Node):
# Create publishers # Create publishers
self.debug_pub = self.create_publisher(String, '/bio/feedback/debug', 10) 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 # Create subscribers
self.control_sub = self.create_subscription(BioControl, '/bio/control', self.send_control, 10) 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 # Topics used in anchor mode
if self.launch_mode == 'anchor': if self.launch_mode == 'anchor':
self.anchor_sub = self.create_subscription(String, '/anchor/bio/feedback', self.anchor_feedback, 10) 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.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
self.bio_feedback = BioFeedback()
# Search for ports IF in 'arm' (standalone) and not 'anchor' mode # Search for ports IF in 'arm' (standalone) and not 'anchor' mode
if self.launch_mode == 'bio': if self.launch_mode == 'bio':
@@ -175,9 +180,20 @@ class SerialRelay(Node):
self.ser.write(bytes(msg, "utf8")) self.ser.write(bytes(msg, "utf8"))
def anchor_feedback(self, msg): def anchor_feedback(self, msg):
output = msg.data
parts = str(output.strip()).split(",")
self.get_logger().info(f"[Bio Anchor] {msg.data}") 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 @staticmethod
def list_serial_ports(): def list_serial_ports():