fix: prevent duplicate NAV2 planning stacks

src/planning_pkg/planning_pkg/planning_manager.py

@@ -6,6 +6,8 @@ from std_msgs.msg import String
 
 import subprocess
 import json
+import os
+import signal
 
 class PlanningManager(Node):
     def __init__(self):
@@ -18,26 +20,59 @@ class PlanningManager(Node):
             reliability=ReliabilityPolicy.RELIABLE
         )
         self.planning_process = None
+        self.planning_config = None
 
         self.create_subscription(String, '/vehicle_config', self.handle_config, qos_profile=qos)
 
     def handle_config(self, msg:String):
-        self.get_logger().info(f"SLAM Received config: {msg.data}")
-        config_data = json.loads(msg.data)
-        
-        self.planning_process = subprocess.Popen(['ros2', 'launch', 
-                                    'planning_pkg', 
-                                    'nav2_planning.launch.py', 
-                                    f'sim_mode:={config_data.get("sim_mode")}', 
-                                    f'robot:={config_data.get("robot")}',
-                                    f'level:={config_data.get("level")}'])
-        pass
+        try:
+            config_data = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().error('Invalid /vehicle_config payload.')
+            return
+
+        config = {
+            'sim_mode': config_data.get('sim_mode'),
+            'robot': config_data.get('robot'),
+            'level': config_data.get('level'),
+        }
+        if self.planning_process and self.planning_process.poll() is None:
+            if config == self.planning_config:
+                self.get_logger().info('Planning stack is already running for this vehicle configuration.')
+                return
+            self.get_logger().warn('Vehicle configuration changed. Restarting the planning stack.')
+            self.stop_planning_process()
+
+        self.get_logger().info(f"Received vehicle config: {config}")
+        self.planning_process = subprocess.Popen(
+            [
+                'ros2', 'launch', 'planning_pkg', 'nav2_planning.launch.py',
+                f'sim_mode:={config["sim_mode"]}',
+                f'robot:={config["robot"]}',
+                f'level:={config["level"]}',
+            ],
+            start_new_session=True,
+        )
+        self.planning_config = config
+
+    def stop_planning_process(self):
+        if not self.planning_process or self.planning_process.poll() is not None:
+            self.planning_process = None
+            return
+
+        try:
+            os.killpg(os.getpgid(self.planning_process.pid), signal.SIGTERM)
+            self.planning_process.wait(timeout=5)
+        except subprocess.TimeoutExpired:
+            os.killpg(os.getpgid(self.planning_process.pid), signal.SIGKILL)
+            self.planning_process.wait()
+        except ProcessLookupError:
+            pass
+        finally:
+            self.planning_process = None
 
     def destroy_node(self):
-        if self.planning_process:
-            self.planning_process.terminate()
-            self.planning_process.wait()
-        
+        self.stop_planning_process()
         return super().destroy_node()
 
 def main(args=None):
@@ -54,4 +89,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-