a

robomaster_pi/launch_node.py

@@ -3,6 +3,7 @@ from rclpy.node import Node
 from std_msgs.msg import String
 from subprocess import Popen, PIPE, STDOUT
 from concurrent.futures import ThreadPoolExecutor
+import threading
 
 class LaunchNode(Node):
 
@@ -15,9 +16,7 @@ class LaunchNode(Node):
 
         # Create a dictionary to store the launched processes
         self.launched_processes = {}
-
-        # Create a thread pool
-        self.thread_pool = ThreadPoolExecutor(max_workers=10)
+        self.process_lock = threading.Lock()  # Lock for thread safety
 
     def on_message(self, msg):
         # Get the launch command and name from the message
@@ -26,6 +25,14 @@ class LaunchNode(Node):
 
         # If the launch command is "kill", send a SIGINT signal to the process with the given name
         if command == "kill":
+            self._terminate_process(name)
+        else:
+            # Otherwise, launch the process with the given command in a separate thread
+            thread = threading.Thread(target=self._launch_process, args=(name, command))
+            thread.start()
+
+    def _terminate_process(self, name):
+        with self.process_lock:
             if name in self.launched_processes:
                 try:
                     process = self.launched_processes[name]
@@ -37,9 +44,11 @@ class LaunchNode(Node):
                     self.get_logger().error(f"Error terminating process '{name}': {e}")
             else:
                 self.get_logger().warn(f"Process '{name}' not found in launched_processes.")
-        else:
+
+    def _launch_process(self, name, command):
+        with self.process_lock:
             try:
-                # Otherwise, launch the process with the given command
+                # Launch the process
                 process = Popen(command, shell=True, stdout=PIPE, stderr=STDOUT)
                 self.launched_processes[name] = process
                 self.get_logger().info(f"Process '{name}' has been launched.")
@@ -59,15 +68,10 @@ def main():
     finally:
         # Terminate all running processes before shutting down
         for name, process in node.launched_processes.items():
-            try:
-                process.send_signal(Popen.SIGINT)
-                process.wait(timeout=5)
-                node.get_logger().info(f"Process '{name}' has been terminated before shutdown.")
-            except Exception as e:
-                node.get_logger().error(f"Error terminating process '{name}': {e}")
+            node._terminate_process(name)
 
         node.destroy_node()
         rclpy.shutdown()
 
 if __name__ == "__main__":
-    main()
\ No newline at end of file
+    main()