From 81558b9f489a99418090d8563f16349c439d0f8a Mon Sep 17 00:00:00 2001 From: Tristan McGinnis Date: Sat, 10 May 2025 12:24:13 -0500 Subject: [PATCH] add core feedback functionality (ANCHOR) --- src/core_pkg/core_pkg/core_node.py | 47 ++++++++++++++++++++++++++---- 1 file changed, 42 insertions(+), 5 deletions(-) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 7ed09ab..f04be98 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -33,6 +33,9 @@ class SerialRelay(Node): self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10) # Create a subscriber self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10) + + # Create a publisher for telemetry + self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback) # Create a service server for pinging the rover self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback) @@ -41,6 +44,8 @@ class SerialRelay(Node): self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10) self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10) + self.core_feedback = CoreFeedback() + if self.launch_mode == 'core': # Loop through all serial devices on the computer to check for the MCU @@ -167,10 +172,6 @@ class SerialRelay(Node): def send_controls(self, msg): #can_relay_tovic,core,19, left_stick, right_stick 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' - #print(f"[Sys] {command}", end="") - - #self.ser.write(bytes(command, "utf8"))# Send command to MCU - #self.get_logger().debug(f"wrote: {command}") self.send_cmd(command) @@ -186,7 +187,43 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) def anchor_feedback(self, msg): - self.get_logger().info(f"[Core Anchor] {msg}") + output = msg.data + parts = str(output.strip()).split(",") + #self.get_logger().info(f"[ANCHOR FEEDBACK parts] {parts}") + if output.startswith("can_relay_fromvic,core,48"): #GNSS Lattitude + self.core_feedback.gps_lat = float(parts[3]) + elif output.startswith("can_relay_fromvic,core,49"):#GNSS Longitude + self.core_feedback.gps_long = float(parts[3]) + elif output.startswith("can_relay_fromvic,core,50"):#GNSS Satellite Count + self.core_feedback.gps_sats = round(float(parts[3])) + elif output.startswith("can_relay_fromvic,core,51"):#Gyro x,y,z + 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]) + 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]) + self.core_feedback.bno_accel.z = float(parts[5]) + self.core_feedback.orientation = float(parts[6]) / 10.0 + elif output.startswith("can_relay_fromvic,core,53"):#Rev motor feedback + pass + #self.updateMotorFeedback(output) + elif output.startswith("can_relay_fromvic,core,54"):#bat, 12, 5, 3, Voltage readings * 100 + self.core_feedback.bat_voltage = float(parts[3]) / 100.0 + self.core_feedback.voltage_12 = float(parts[4]) / 100.0 + self.core_feedback.voltage_5 = float(parts[5]) / 100.0 + self.core_feedback.voltage_3 = float(parts[6]) / 100.0 + elif output.startswith("can_relay_fromvic,core,56"):#BMP Temp, Altitude, Pressure + self.core_feedback.bmp_temp = float(parts[3]) + self.core_feedback.bmp_alt = float(parts[4]) + self.core_feedback.bmp_pres = float(parts[5]) + else: + return + #self.get_logger().info(f"[Core Anchor] {msg}") + + def publish_feedback(self): + #self.get_logger().info(f"[Core] {self.core_feedback}") + self.feedback_pub.publish(self.core_feedback) def ping_callback(self, request, response): return response