Skip to content
Snippets Groups Projects
Commit f5d3cd30 authored by Emil Harlan's avatar Emil Harlan
Browse files

a

parent a1dd5cfa
No related branches found
No related tags found
No related merge requests found
......@@ -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()
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment