diff --git a/robomaster_pi/launch_node.py b/robomaster_pi/launch_node.py index 9cb0461eb3fb1b80e3e3a0102824f17099a5ec0c..7c01709a0057560dd9bc59ab50f72ced4b6646d9 100644 --- a/robomaster_pi/launch_node.py +++ b/robomaster_pi/launch_node.py @@ -2,13 +2,11 @@ import rclpy 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): +class LaunchControlNode(Node): def __init__(self): - super().__init__("launch_node") + super().__init__("launch_control_node") # Subscribe to the launch_node topic self.subscription = self.create_subscription( @@ -16,7 +14,6 @@ class LaunchNode(Node): # Create a dictionary to store the launched processes self.launched_processes = {} - self.process_lock = threading.Lock() # Lock for thread safety def on_message(self, msg): # Get the launch command and name from the message @@ -27,51 +24,39 @@ class LaunchNode(Node): 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() + # Otherwise, launch the process with the given command + self._launch_process(name, command) def _terminate_process(self, name): - with self.process_lock: - if name in self.launched_processes: - try: - process = self.launched_processes[name] - process.send_signal(Popen.SIGINT) - process.wait(timeout=5) # Wait for the process to terminate (timeout: 5 seconds) - del self.launched_processes[name] - self.get_logger().info(f"Process '{name}' has been terminated.") - except Exception as e: - self.get_logger().error(f"Error terminating process '{name}': {e}") - else: - self.get_logger().warn(f"Process '{name}' not found in launched_processes.") - - def _launch_process(self, name, command): - with self.process_lock: + if name in self.launched_processes: try: - # 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.") - # Read and log the output of the process - for line in process.stdout: - self.get_logger().info(f"Process '{name}' output: {line.decode().strip()}") + process = self.launched_processes[name] + process.send_signal(Popen.SIGINT) + process.wait(timeout=5) # Wait for the process to terminate (timeout: 5 seconds) + del self.launched_processes[name] + self.get_logger().info(f"Process '{name}' has been terminated.") except Exception as e: - self.get_logger().error(f"Error launching process '{name}': {e}") + self.get_logger().error(f"Error terminating process '{name}': {e}") + else: + self.get_logger().warn(f"Process '{name}' not found in launched_processes.") + + def _launch_process(self, name, command): + try: + # 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.") + # Read and log the output of the process + for line in process.stdout: + self.get_logger().info(f"Process '{name}' output: {line.decode().strip()}") + except Exception as e: + self.get_logger().error(f"Error launching process '{name}': {e}") def main(): rclpy.init() - node = LaunchNode() - try: - rclpy.spin(node) - except KeyboardInterrupt: - pass - finally: - # Terminate all running processes before shutting down - for name, process in node.launched_processes.items(): - node._terminate_process(name) - - node.destroy_node() - rclpy.shutdown() + node = LaunchControlNode() + rclpy.spin(node) + rclpy.shutdown() if __name__ == "__main__": main()