add launch mode logic

src/core_pkg/core_pkg/core_node.py

@@ -23,41 +23,50 @@ class SerialRelay(Node):
         # Initalize node with name
         super().__init__("core_node")#previously 'serial_publisher'
 
-        # Create publishers for feedback and telemetry
+        # Get launch mode parameter
+        self.declare_parameter('launch_mode', 'core')
+        launch_mode = self.get_parameter('launch_mode').value
+        self.get_logger().info(f"core launch_mode is: {launch_mode}")
+
+        # Create publishers 
         self.debug_pub = self.create_publisher(String, '/core/debug', 10) 
         self.feedback_pub = self.create_publisher(CoreFeedback, '/core/feedback', 10)
-
-        # Create a subscriber to listen to any commands sent for the MCU
+        # Create a subscriber 
         self.control_sub = self.create_subscription(CoreControl, '/core/control', self.send_controls, 10)
-        
 
         # Create a service server for pinging the rover
         self.ping_service = self.create_service(Empty, '/astra/core/ping', self.ping_callback)
 
-        # Loop through all serial devices on the computer to check for the MCU
-        self.port = None
-        ports = SerialRelay.list_serial_ports()
-        for i in range(2):
-            for port in ports:
-                try:
-                    # connect and send a ping command
-                    ser = serial.Serial(port, 115200, timeout=1)
-                    print(f"Checking port {port}...")
-                    ser.write(b"ping\n")
-                    response = ser.read_until("\n")
+        if launch_mode == 'anchor':
+            self.anchor_sub = self.create_subscription(String, '/anchor/core/feedback', self.anchor_feedback, 10)
+            self.anchor_pub = self.create_publisher(String, '/anchor/relay', 10)
 
-                    # if pong is in response, then we are talking with the MCU
-                    if b"pong" in response:
-                        self.port = port
-                        print(f"Found MCU at {self.port}!")
-                        break
-                except:
-                    pass
-            if self.port is not None:
-                break
+
+        if launch_mode == 'core':
+            # Loop through all serial devices on the computer to check for the MCU
+            self.port = None
+            ports = SerialRelay.list_serial_ports()
+            for i in range(2):
+                for port in ports:
+                    try:
+                        # connect and send a ping command
+                        ser = serial.Serial(port, 115200, timeout=1)
+                        #(f"Checking port {port}...")
+                        ser.write(b"ping\n")
+                        response = ser.read_until("\n")
+
+                        # if pong is in response, then we are talking with the MCU
+                        if b"pong" in response:
+                            self.port = port
+                            self.get_logger.info(f"Found MCU at {self.port}!")
+                            break
+                    except:
+                        pass
+                if self.port is not None:
+                    break
         
         if self.port is None:
-            print("Unable to find MCU...")
+            self.get_logger.info("Unable to find MCU...")
             time.sleep(1)
             sys.exit(1)
         
@@ -71,11 +80,12 @@ class SerialRelay(Node):
         thread = threading.Thread(target=rclpy.spin, args={self}, daemon=True)
         thread.start()
         
-        try:
-            while rclpy.ok():
-                self.read_MCU() # Check the MCU for updates
-        except KeyboardInterrupt:
-            sys.exit(0)
+        if self.launch_mode == 'core':
+            try:
+                while rclpy.ok():
+                    self.read_MCU() # Check the MCU for updates
+            except KeyboardInterrupt:
+                sys.exit(0)
 
     def read_MCU(self):
         try:
@@ -157,10 +167,18 @@ class SerialRelay(Node):
         command = "can_relay_tovic,core,19," + self.scale_duty(left_stick_neg, msg.max_speed) + ',' + self.scale_duty(msg.right_stick, msg.max_speed) + '\n'
         #print(f"[Sys] {command}", end="")
         
-        self.ser.write(bytes(command, "utf8"))# Send command to MCU
-        self.get_logger().debug(f"wrote: {command}")
+        #self.ser.write(bytes(command, "utf8"))# Send command to MCU
+        #self.get_logger().debug(f"wrote: {command}")
+        
+        self.send_cmd(command)
+        
         #print(f"[Sys] Relaying: {command}")
 
+    def send_cmd(self, cmd):
+        if self.launch_mode == 'anchor':
+            self.anchor_pub.publish(cmd)
+        elif self.launch_mode == 'core':
+            self.ser.write(bytes(cmd, "utf8"))
 
     def ping_callback(self, request, response):
         return response