refactor: post-comp IK testing

src/core_pkg/core_pkg/core_node.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from rclpy import qos
 from std_srvs.srv import Empty
 
 import signal
@@ -19,6 +20,17 @@ from ros2_interfaces_pkg.msg import CoreControl
 serial_pub = None
 thread = None
 
+control_qos = qos.QoSProfile(
+    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    deadline=1000,
+    lifespan=500,
+    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    liveliness_lease_duration=5000
+)
+
 class SerialRelay(Node):
     def __init__(self):
         # Initalize node with name
@@ -33,7 +45,7 @@ class SerialRelay(Node):
         self.debug_pub = self.create_publisher(String, '/core/debug', 10) 
         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)
+        self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, qos_profile=control_qos)
         
         # Create a publisher for telemetry
         self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
@@ -75,9 +87,8 @@ class SerialRelay(Node):
         
             if self.port is None:
                 self.get_logger().info("Unable to find MCU...")
-                #kill the node/process entirely
-                os.kill(os.getpid(), signal.SIGKILL)
-                sys.exit(0)
+                time.sleep(1)
+                sys.exit(1)
         
             self.ser = serial.Serial(self.port, 115200)
             atexit.register(self.cleanup)
@@ -265,8 +276,11 @@ class SerialRelay(Node):
     
     def cleanup(self):
         print("Cleaning up before terminating...")
-        if self.ser.is_open:
-            self.ser.close()
+        try:
+            if self.ser.is_open:
+                self.ser.close()
+        except Exception as e:
+            exit(0)
 
 def myexcepthook(type, value, tb):
     print("Uncaught exception:", type, value)