a

startup/startup.py

 import yaml
 import os
 import subprocess
+import signal
+import sys
+
+ros_nodes = []
 
 def check_mapping_value():
     yaml_file = "startup_params.yaml"
@@ -12,22 +16,9 @@ def check_mapping_value():
         mapping_value = params.get("mapping")
         if mapping_value is not None and isinstance(mapping_value, bool):
             if mapping_value:
-                try:
-                    
-                    subprocess.Popen("ros2 launch robot_loc robot.launch.py", shell=True)
-                    subprocess.Popen("ros2 launch robot_loc mapping_launch.py", shell=True)
-                    subprocess.Popen("ros2 launch robot_loc navigation_launch.py", shell=True)
-                    print("ROS node started as a subprocess.")
-                except Exception as e:
-                    print(f"Failed to start ROS node as a subprocess: {e}")
+                start_mapping_nodes()
             else:
-                try:
-                    subprocess.Popen("ros2 launch robot_loc robot.launch.py", shell=True)
-                    subprocess.Popen("ros2 launch robot_loc localization_launch.py map:=./Hackspace.yaml'", shell=True)
-                    subprocess.Popen("ros2 launch robot_loc navigation_launch.py map_subscribe_transient_local:=true", shell=True)
-                    print("ROS node started as a subprocess.")
-                except Exception as e:
-                    print(f"Failed to start ROS node as a subprocess: {e}")
+                start_localization_nodes()
         else:
             print("Invalid or missing 'mapping' parameter in the YAML file.")
     
@@ -38,5 +29,61 @@ def check_mapping_value():
     except Exception as e:
         print(f"An error occurred: {e}")
 
+def start_mapping_nodes():
+    try:
+        robot_node = subprocess.Popen("ros2 launch robot_loc robot.launch.py", shell=True)
+        mapping_node = subprocess.Popen("ros2 launch robot_loc mapping_launch.py", shell=True)
+        navigation_node = subprocess.Popen("ros2 launch robot_loc navigation_launch.py", shell=True)
+
+        # Append the node processes to the ros_nodes list
+        ros_nodes.extend([robot_node, mapping_node, navigation_node])
+
+        print("Mapping nodes started.")
+        while True:
+            # Keep the script running as long as the nodes are running
+            for node in ros_nodes:
+                if node.poll() is not None:  # If the process has finished
+                    print("One or more nodes have exited. Exiting...")
+                    cleanup()
+                    sys.exit(0)
+    except Exception as e:
+        print(f"Failed to start mapping nodes: {e}")
+
+def start_localization_nodes():
+    try:
+        robot_node = subprocess.Popen("ros2 launch robot_loc robot.launch.py", shell=True)
+        localization_node = subprocess.Popen("ros2 launch robot_loc localization_launch.py map:=./Hackspace.yaml", shell=True)
+        navigation_node = subprocess.Popen("ros2 launch robot_loc navigation_launch.py map_subscribe_transient_local:=true", shell=True)
+
+        # Append the node processes to the ros_nodes list
+        ros_nodes.extend([robot_node, localization_node, navigation_node])
+
+        print("Localization nodes started.")
+        while True:
+            # Keep the script running as long as the nodes are running
+            for node in ros_nodes:
+                if node.poll() is not None:  # If the process has finished
+                    print("One or more nodes have exited. Exiting...")
+                    cleanup()
+                    sys.exit(0)
+    except Exception as e:
+        print(f"Failed to start localization nodes: {e}")
+
+def cleanup():
+    # Terminate all ROS node processes gracefully before exiting the script
+    for node in ros_nodes:
+        try:
+            node.terminate()
+        except Exception as e:
+            print(f"Failed to terminate ROS node process: {e}")
+
+def signal_handler(sig, frame):
+    print("Ctrl+C detected. Exiting...")
+    cleanup()
+    sys.exit(0)
+
 if __name__ == "__main__":
+    # Set up the signal handler for Ctrl+C
+    signal.signal(signal.SIGINT, signal_handler)
+
     check_mapping_value()