Fixed rogue thread issue on integrate_database

Author: fcladera

mocha_core/mocha_core/integrate_database.py

@@ -22,7 +22,6 @@
 import mocha_core.database_server as ds
 import mocha_core.database_utils as du
 import mocha_core.synchronize_channel as sync
-from mocha_core.srv import AddUpdateDB, GetDataHeaderDB, SelectDB
 
 
 def ping(host):
@@ -41,13 +40,11 @@ def __init__(self):
         super().__init__("integrate_database")
 
         # Handle shutdown signal
-        self.interrupted = False
         self.shutdownTriggered = threading.Event()
         self.shutdownTriggered.clear()
+
         def signal_handler(sig, frame):
-            self.logger.warning(f"{self.this_robot} - Integrate - " +
-                               f"Got SIGINT. Triggering shutdown.")
-            self.interrupted = True
+            self.logger.warning(f"{self.this_robot} - Integrate - Got SIGINT. Triggering shutdown.")
             self.shutdown("Killed by user")
         signal.signal(signal.SIGINT, signal_handler)
 
@@ -66,7 +63,8 @@ def signal_handler(sig, frame):
         self.rssi_threshold = self.get_parameter("rssi_threshold").get_parameter_value().integer_value
 
         if len(self.this_robot) == 0:
-            self.shutdown("Empty robot name")
+            self.logger.error(f"{self.this_robot} - Integrate - Empty robot name")
+            raise ValueError("Empty robot name")
 
         self.logger.info(f"{self.this_robot} - Integrate - " +
                         f"RSSI threshold: {self.rssi_threshold}")
@@ -80,39 +78,44 @@ def signal_handler(sig, frame):
             with open(self.robot_configs_file, "r") as f:
                 self.robot_configs = yaml.load(f, Loader=yaml.FullLoader)
         except Exception as e:
-            self.shutdown(f"Error opening robot_configs file: {e}")
+            self.logger.error(f"{self.this_robot} - Integrate - robot_configs file")
+            raise e
         if self.this_robot not in self.robot_configs.keys():
-            self.shutdown("Robot not in config file")
+            self.logger.error(f"{self.this_robot} - Integrate - robot_configs file")
+            raise ValueError("Robot not in config file")
 
         # Load and check radio configs
         self.radio_configs_file = self.get_parameter("radio_configs").get_parameter_value().string_value
         try:
             with open(self.radio_configs_file, "r") as f:
                 self.radio_configs = yaml.load(f, Loader=yaml.FullLoader)
         except Exception as e:
-            self.shutdown(f"Error opening radio_configs file: {e}")
+            self.logger.error(f"{self.this_robot} - Integrate - radio_configs file")
+            raise e
         self.radio = self.robot_configs[self.this_robot]["using-radio"]
         if self.radio not in self.radio_configs.keys():
-            self.shutdown("Radio {self.radio} not in config file")
+            self.logger.error(f"{self.this_robot} - Integrate - radio_configs file")
+            raise ValueError("Radio {self.radio} not in config file")
 
         # Load and check topic configs
         self.topic_configs_file = self.get_parameter("topic_configs").get_parameter_value().string_value
         try:
             with open(self.topic_configs_file, "r") as f:
                 self.topic_configs = yaml.load(f, Loader=yaml.FullLoader)
         except Exception as e:
-            self.shutdown(f"Error opening topic_configs file: {e}")
+            self.logger.error(f"{self.this_robot} - Integrate - topics_configs file")
+            raise e
         self_type = self.robot_configs[self.this_robot]["node-type"]
         if self_type not in self.topic_configs.keys():
-            self.shutdown("Node type not in config file")
+            self.logger.error(f"{self.this_robot} - Integrate - topics_configs file")
+            raise ValueError("Node type not in config file")
 
         # Check that we can ping the radios
         ip = self.robot_configs[self.this_robot]["IP-address"]
         if not ping(ip):
             self.logger.error(f"{self.this_robot} - Integrate - " +
                              f"Cannot ping self {ip}. Is the radio on?")
-            self.shutdown("Cannot ping self")
-            return
+            raise ValueError("Cannot ping self")
 
         # Create database server
         self.DBServer = ds.DatabaseServer(self.robot_configs,
@@ -141,8 +144,8 @@ def signal_handler(sig, frame):
                                    self.this_robot,
                                    other_robot, self.robot_configs,
                                    self.client_timeout, self)
-            self.all_channels.append(channel)
             channel.run()
+            self.all_channels.append(channel)
 
             # Attach a radio trigger to each channel. This will be triggered
             # when the RSSI is high enough. You can use another approach here
@@ -157,23 +160,9 @@ def make_callback(ch):
                 10
             )
 
-        # Wait for all the robots to start
-        for _ in range(100):
-            if self.interrupted or not rclpy.ok():
-                self.shutdown("Killed while waiting for other robots")
-                return
-            time.sleep(0.1)
-        # Node is ready
-        self.mtexecutor = MultiThreadedExecutor(num_threads=4)
-        self.mtexecutor.add_node(self)
-
-    def run(self):
-        self.mtexecutor.spin()
-
     def shutdown(self, reason):
         # Only trigger shutdown once
         if self.shutdownTriggered.is_set():
-            sys.exit(1)
             return
         self.shutdownTriggered.set()
 
@@ -183,21 +172,11 @@ def shutdown(self, reason):
         if hasattr(self, 'all_channels') and len(self.all_channels) != 0:
             for channel in self.all_channels:
                 channel.stop()
-                self.all_channels.remove(channel)
             self.logger.warning(f"{self.this_robot} - Integrate - " + "Killed Channels")
             # Wait for all the channels to be gone. This needs to be slightly
             # larger than RECV_TIMEOUT
             time.sleep(3.5)
-
-        pdb.set_trace()
-
-        # Stop the executor to exit the spin loop
-        if hasattr(self, 'mtexecutor') and self.mtexecutor is not None:
-            self.mtexecutor.shutdown()
-            self.logger.warning(f"{self.this_robot} - Integrate - " + "Executor shut down")
-        self.destroy_node()
-        self.logger.warning(f"{self.this_robot} - Integrate - " + "Node destroyed")
-        sys.exit(1)
+        self.logger.warning(f"{self.this_robot} - Integrate - " + "Shutdown complete")
 
     def rssi_cb(self, data, comm_node):
         rssi = data.data
@@ -217,19 +196,30 @@ def main(args=None):
     # Start the node
     try:
         integrate_db = IntegrateDatabase()
-    except RuntimeError as e:
+    except Exception as e:
         print(f"Node initialization failed: {e}")
+        rclpy.shutdown()
         return
 
+    # Load mtexecutor
+    mtexecutor = MultiThreadedExecutor(num_threads=4)
+    mtexecutor.add_node(integrate_db)
+
+    # Use context manager for clean shutdown
     try:
-        integrate_db.run()
+        # Spin with periodic checking for shutdown
+        while rclpy.ok() and not integrate_db.shutdownTriggered.is_set():
+            mtexecutor.spin_once(timeout_sec=0.1)
     except KeyboardInterrupt:
         print("Keyboard Interrupt")
-        pass
+        integrate_db.shutdown("KeyboardInterrupt")
     except Exception as e:
-        print(e)
+        print(f"Exception: {e}")
+        integrate_db.shutdown(f"Exception: {e}")
     finally:
-        integrate_db.shutdown("Shutting down")
+        # Clean up node and ROS2 from main thread (safe)
+        integrate_db.destroy_node()
+        rclpy.shutdown()
 
 if __name__ == "__main__":
     main()

mocha_core/mocha_core/zmq_comm_node.py

@@ -82,7 +82,8 @@ def __init__(self, this_node, client_node, robot_configs,
 
         # Start server thread
         self.th = threading.Thread(target=self.server_thread, args=())
-        self.server_running = True
+        self.server_running = threading.Event()
+        self.server_running.clear()
         self.th.start()
 
     def connect_send_message(self, msg):
@@ -185,6 +186,7 @@ def connect_send_message(self, msg):
         self.syncStatus_lock.release()
 
     def server_thread(self):
+        self.server_running.set()
         # This timer does not have a big impact as it is only the timer until
         # the recv times out Most calls from the client are very lightweight
         RECV_TIMEOUT = 1000
@@ -195,7 +197,7 @@ def server_thread(self):
 
         self.server.bind("tcp://*:" + str(port))
 
-        while self.server_running:
+        while self.server_running.is_set():
             try:
                 request = self.server.recv()
             except zmq.ZMQError as e:
@@ -230,6 +232,6 @@ def server_thread(self):
         self.context.term()
 
     def terminate(self):
-        self.server_running = False
+        self.server_running.clear()
         self.th.join()
         self.logger.warn(f"Node {self.this_node} <- {self.client_node} - Terminating server")