diff --git a/src/anchor_pkg/anchor_pkg/anchor_node.py b/src/anchor_pkg/anchor_pkg/anchor_node.py index 6b06947..8edc8b5 100644 --- a/src/anchor_pkg/anchor_pkg/anchor_node.py +++ b/src/anchor_pkg/anchor_pkg/anchor_node.py @@ -1,9 +1,9 @@ +from warnings import deprecated import rclpy from rclpy.node import Node from rclpy.executors import ExternalShutdownException from rcl_interfaces.msg import ParameterDescriptor, ParameterType -import signal import atexit from .connector import ( @@ -14,7 +14,7 @@ from .connector import ( NoValidDeviceException, NoWorkingDeviceException, ) -from .convert import string_to_viccan, viccan_to_string +from .convert import string_to_viccan import threading from astra_msgs.msg import VicCAN @@ -163,6 +163,12 @@ class Anchor(Node): self.write_connector, 20, ) + self.tovic_sub_legacy_ = self.create_subscription( + String, + "/anchor/relay", + self.write_connector_legacy, + 20, + ) self.mock_mcu_sub_ = self.create_subscription( String, "/anchor/from_vic/mock_mcu", @@ -195,7 +201,21 @@ class Anchor(Node): def write_connector(self, msg: VicCAN): """Write to the connector and send a copy to /anchor/to_vic/debug""" self.connector.write(msg) - self.tovic_debug_pub_.publish(viccan_to_string(msg)) + self.tovic_debug_pub_.publish(msg) + + @deprecated("Use /anchor/to_vic/relay instead of /anchor/relay") + def write_connector_legacy(self, msg: String): + """Write to the connector by first attempting to convert String to VicCAN""" + # please do not reference this code. ~riley + for cmd in msg.data.split("\n"): + viccan = string_to_viccan( + cmd, + "anchor", + self.get_logger(), + self.get_clock().now().to_msg(), + ) + if viccan: + self.write_connector(viccan) def relay_fromvic(self, msg: VicCAN): """Relay a message from the MCU to the appropriate VicCAN topic""" diff --git a/src/anchor_pkg/anchor_pkg/connector.py b/src/anchor_pkg/anchor_pkg/connector.py index 88e17d8..58c33d9 100644 --- a/src/anchor_pkg/anchor_pkg/connector.py +++ b/src/anchor_pkg/anchor_pkg/connector.py @@ -217,6 +217,7 @@ class CANConnector(Connector): # filter to busses whose channel matches the can_override if can_override: + self.logger.info(f"overrode can interface with {can_override}") avail = list( filter( lambda b: b.get("channel") == can_override, diff --git a/src/anchor_pkg/anchor_pkg/convert.py b/src/anchor_pkg/anchor_pkg/convert.py index 49a4bda..cfecfce 100644 --- a/src/anchor_pkg/anchor_pkg/convert.py +++ b/src/anchor_pkg/anchor_pkg/convert.py @@ -16,7 +16,7 @@ def string_to_viccan( parts: list[str] = msg.strip().split(",") # don't need an extra check because len of .split output is always >= 1 - if parts[0] != "can_relay_fromvic": + if not parts[0].startswith("can_relay_"): logger.debug(f"got non-CAN data from {mcu_name}: {msg}") return None elif len(parts) < 3: @@ -59,4 +59,4 @@ def viccan_to_string(viccan: VicCAN) -> str: """Converts a ROS2 VicCAN message to the serial string VicCAN format.""" # go from [ w, x, y, z ] -> ",w,x,y,z" & round to 7 digits max data = "".join([f",{round(val,7)}" for val in viccan.data]) - return f"can_relay_tovic,{viccan.mcu_name}{data}\n" + return f"can_relay_tovic,{viccan.mcu_name},{viccan.command_id}{data}\n" diff --git a/src/anchor_pkg/launch/rover.launch.py b/src/anchor_pkg/launch/rover.launch.py index 824ddd3..7c987dc 100644 --- a/src/anchor_pkg/launch/rover.launch.py +++ b/src/anchor_pkg/launch/rover.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): ld.add_action( DeclareLaunchArgument( "can_override", - default_value="auto", + default_value="", description="CAN network override parameter for anchor node (default: '')", ) )