diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 01d283a..3c19b8e 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -11,6 +11,7 @@ import signal from std_msgs.msg import String from sensor_msgs.msg import JointState from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual +from astra_msgs.msg import ArmFeedback, VicCAN import math # control_qos = qos.QoSProfile( @@ -27,6 +28,14 @@ import math serial_pub = None thread = None +# Used to verify the length of an incoming VicCAN feedback message +# Key is VicCAN command_id, value is expected length of data list +viccan_msg_len_dict = { + 53: 4, + 54: 4, + 55: 4 +} + class SerialRelay(Node): def __init__(self): @@ -42,6 +51,13 @@ class SerialRelay(Node): # Anchor topics if self.launch_mode == "anchor": + self.anchor_fromvic_sub_ = self.create_subscription( + VicCAN, "/anchor/from_vic/arm", self.relay_fromvic, 20 + ) + self.anchor_tovic_pub_ = self.create_publisher( + VicCAN, "/anchor/to_vic/relay", 20 + ) + self.anchor_sub = self.create_subscription( String, "/anchor/arm/feedback", self.anchor_feedback, 10 ) @@ -255,6 +271,21 @@ class SerialRelay(Node): else: return + def relay_fromvic(self, msg: VicCAN): + # Assume that the message is coming from Core + # skill diff if not + + # Check message len to prevent crashing on bad data + if msg.command_id in viccan_msg_len_dict: + expected_len = viccan_msg_len_dict[msg.command_id] + if len(msg.data) != expected_len: + self.get_logger().warning( + f"Ignoring VicCAN message with id {msg.command_id} due to unexpected data length (expected {expected_len}, got {len(msg.data)})" + ) + return + + # match msg.command_id: + def publish_feedback(self): self.socket_pub.publish(self.arm_feedback) self.digit_pub.publish(self.digit_feedback)