diff --git a/robomaster_pi/launch_node.py b/robomaster_pi/launch_node.py index 11ee99c6db944ea1cd045d23119d3fd852c83fcf..9cb0461eb3fb1b80e3e3a0102824f17099a5ec0c 100644 --- a/robomaster_pi/launch_node.py +++ b/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()