From a1dd5cfa9b5140fe11a92e30e86d14b86aadc23f Mon Sep 17 00:00:00 2001
From: Emil Harlan <eharlan@uni-bremen.de>
Date: Mon, 24 Jul 2023 09:31:38 +0200
Subject: [PATCH] a
---
robomaster_pi/launch_node.py | 51 ++++++++++++++++++++++++++----------
1 file changed, 37 insertions(+), 14 deletions(-)
diff --git a/robomaster_pi/launch_node.py b/robomaster_pi/launch_node.py
index 2e9f988..11ee99c 100644
--- a/robomaster_pi/launch_node.py
+++ b/robomaster_pi/launch_node.py
@@ -1,10 +1,8 @@
import rclpy
from rclpy.node import Node
-from rclpy.subscription import Subscription
from std_msgs.msg import String
-from subprocess import Popen
+from subprocess import Popen, PIPE, STDOUT
from concurrent.futures import ThreadPoolExecutor
-import os
class LaunchNode(Node):
@@ -29,22 +27,47 @@ class LaunchNode(Node):
# If the launch command is "kill", send a SIGINT signal to the process with the given name
if command == "kill":
if name in self.launched_processes:
- os.kill(self.launched_processes[name], signal.SIGINT)
- del self.launched_processes[name]
+ 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.")
else:
- # Otherwise, launch the process with the given command
- self.thread_pool.submit(self.launch_process, name, command)
-
- def launch_process(self, name, command):
- # Launch the process
- process = Popen(command)
- self.launched_processes[name] = process
+ try:
+ # Otherwise, launch the process with the given command
+ 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()
- rclpy.spin(node)
- rclpy.shutdown()
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ 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.destroy_node()
+ rclpy.shutdown()
if __name__ == "__main__":
main()
\ No newline at end of file
--
GitLab