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

refactor: change Core commands to VicCAN

Browse Source
This commit is contained in:
David
2025-09-29 10:29:01 -05:00
parent a25983126f
commit b1f4fe8320
2 changed files with 21 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/anchor_pkg/anchor_pkg/anchor_node.py
View File

@@ -12,7 +12,7 @@ import sys
import threading import threading
import glob import glob
from std_msgs.msg import String from std_msgs.msg import String, Header
from ros2_interfaces_pkg.msg import VicCAN from ros2_interfaces_pkg.msg import VicCAN
serial_pub = None serial_pub = None
@@ -132,16 +132,16 @@ class SerialRelay(Node):
def relay_mock_fromvic(self, msg: String): 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) self.relay_fromvic(msg.data)
def relay_tovic(self, msg: VicCAN): def relay_tovic(self, msg: VicCAN):
output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}" output: str = f"can_relay_tovic,{msg.mcu_name},{msg.command_id}"
for num in msg.data: for num in msg.data:
output += f",{num}" output += f",{round(num, 7)}" # limit to 7 decimal places
output += "\n" 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")) self.ser.write(bytes(output, "utf8"))
def relay_fromvic(self, msg: str): def relay_fromvic(self, msg: str):
@@ -149,12 +149,14 @@ class SerialRelay(Node):
parts = msg.strip().split(",") parts = msg.strip().split(",")
if len(parts) < 3 or parts[0] != "can_relay_fromvic": if len(parts) < 3 or parts[0] != "can_relay_fromvic":
return return
output = VicCAN() output = VicCAN()
output.mcu_name = parts[1] output.mcu_name = parts[1]
output.command_id = int(parts[2]) output.command_id = int(parts[2])
if len(parts) > 3: if len(parts) > 3:
output.data = [float(x) for x in 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}") # self.get_logger().info(f"Relaying from MCU: {output}")
if output.mcu_name == "core": if output.mcu_name == "core":
self.fromvic_core_pub_.publish(output) self.fromvic_core_pub_.publish(output)

20
src/core_pkg/core_pkg/core_node.py
View File

@@ -15,7 +15,7 @@ import threading
import glob import glob
from scipy.spatial.transform import Rotation 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 sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
from geometry_msgs.msg import TwistStamped, Twist from geometry_msgs.msg import TwistStamped, Twist
from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback from ros2_interfaces_pkg.msg import CoreControl, CoreFeedback
@@ -236,8 +236,7 @@ class SerialRelay(Node):
vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO 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 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_viccan(20, [vel_left_rpm, vel_right_rpm])
self.send_cmd(command)
def twist_man_callback(self, msg: Twist): def twist_man_callback(self, msg: Twist):
linear = msg.linear.x # [-1 1] for forward/back from left stick y 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_left = map_range(duty_left, -1, 1, -FAST_SPEED, FAST_SPEED)
duty_right = map_range(duty_right, -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_viccan(19, [duty_left, duty_right])
self.send_cmd(command)
def control_state_callback(self, msg: CoreCtrlState): def control_state_callback(self, msg: CoreCtrlState):
# Brake mode # Brake mode
command = f"can_relay_tovic,core,18,{str(int(msg.brake_mode))}\n" self.send_viccan(18, [msg.brake_mode])
self.send_cmd(command)
# Speed mode # Speed mode
self.twist_speed_mode = msg.speed_mode # twist_man_callback will handle this 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")) 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): def anchor_feedback(self, msg: String):
output = msg.data output = msg.data
parts = str(output.strip()).split(",") parts = str(output.strip()).split(",")