From b1f4fe8320d0baeeec18730fd386e1da00db1c7a Mon Sep 17 00:00:00 2001 From: David Date: Mon, 29 Sep 2025 10:29:01 -0500 Subject: [PATCH] refactor: change Core commands to VicCAN --- src/anchor_pkg/anchor_pkg/anchor_node.py | 12 +++++++----- src/core_pkg/core_pkg/core_node.py | 22 ++++++++++++++-------- 2 files changed, 21 insertions(+), 13 deletions(-) diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 0af5ffa..c07ff2c 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -12,7 +12,7 @@ import sys import threading import glob -from std_msgs.msg import String +from std_msgs.msg import String, Header from ros2_interfaces_pkg.msg import VicCAN serial_pub = None @@ -132,16 +132,16 @@ class SerialRelay(Node): def relay_mock_fromvic(self, msg: String): - #self.get_logger().info(f"Got command from mock MCU: {msg}") + # self.get_logger().info(f"Got command from mock MCU: {msg}") self.relay_fromvic(msg.data) def relay_tovic(self, msg: VicCAN): output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}" for num in msg.data: - output += f",{num}" + output += f",{round(num, 7)}" # limit to 7 decimal places output += "\n" - #self.get_logger().info(f"Relaying to MCU: {output}") + # self.get_logger().info(f"VicCAN relay to MCU: {output}") self.ser.write(bytes(output, "utf8")) def relay_fromvic(self, msg: str): @@ -149,12 +149,14 @@ class SerialRelay(Node): parts = msg.strip().split(",") if len(parts) < 3 or parts[0] != "can_relay_fromvic": return + output = VicCAN() output.mcu_name = parts[1] output.command_id = int(parts[2]) if len(parts) > 3: output.data = [float(x) for x in parts[3:]] - output.header.stamp = self.get_clock().now().to_msg() + output.header = Header(stamp=self.get_clock().now().to_msg(), frame_id="from_vic") + # self.get_logger().info(f"Relaying from MCU: {output}") if output.mcu_name == "core": self.fromvic_core_pub_.publish(output) diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index 13fbb58..8e97180 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -15,7 +15,7 @@ import threading import glob from scipy.spatial.transform import Rotation -from std_msgs.msg import String +from std_msgs.msg import String, Header from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus from geometry_msgs.msg import TwistStamped, Twist from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback @@ -235,9 +235,8 @@ class SerialRelay(Node): vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO - - command = f"can_relay_tovic,core,20,{vel_left_rpm},{vel_right_rpm}\n" - self.send_cmd(command) + + self.send_viccan(20, [vel_left_rpm, vel_right_rpm]) def twist_man_callback(self, msg: Twist): linear = msg.linear.x # [-1 1] for forward/back from left stick y @@ -269,13 +268,11 @@ class SerialRelay(Node): duty_left = map_range(duty_left, -1, 1, -FAST_SPEED, FAST_SPEED) duty_right = map_range(duty_right, -1, 1, -FAST_SPEED, FAST_SPEED) - command = f"can_relay_tovic,core,19,{duty_left},{duty_right}\n" - self.send_cmd(command) + self.send_viccan(19, [duty_left, duty_right]) def control_state_callback(self, msg: CoreCtrlState): # Brake mode - command = f"can_relay_tovic,core,18,{str(int(msg.brake_mode))}\n" - self.send_cmd(command) + self.send_viccan(18, [msg.brake_mode]) # Speed mode self.twist_speed_mode = msg.speed_mode # twist_man_callback will handle this @@ -291,6 +288,15 @@ class SerialRelay(Node): self.ser.write(bytes(msg, "utf8")) + def send_viccan(self, cmd_id: int, data: list[float]): + self.anchor_tovic_pub_.publish(VicCAN( + header=Header(stamp=self.get_clock().now().to_msg(), frame_id="to_vic"), + mcu_name="core", + command_id=cmd_id, + data=data + )) + + def anchor_feedback(self, msg: String): output = msg.data parts = str(output.strip()).split(",")