fix control stack startup and duplicate config handling

src/control_pkg/control_pkg/control_manager.py

@@ -6,6 +6,8 @@ from std_msgs.msg import String
 
 import subprocess
 import json
+import os
+import signal
 
 class ControlManager(Node):
     def __init__(self):
@@ -17,27 +19,60 @@ class ControlManager(Node):
             durability=DurabilityPolicy.TRANSIENT_LOCAL,
             reliability=ReliabilityPolicy.RELIABLE
         )
-        self.planning_process = None
+        self.control_process = None
+        self.control_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"Control Received config: {msg.data}")
+        try:
             config_data = json.loads(msg.data)
+        except json.JSONDecodeError:
+            self.get_logger().error('Invalid /vehicle_config payload.')
+            return
 
-        self.planning_process = subprocess.Popen(['ros2', 'launch', 
-                                    'control_pkg', 
-                                    'nav2_control.launch.py', 
-                                    f'sim_mode:={config_data.get("sim_mode")}', 
-                                    f'robot:={config_data.get("robot")}',
-                                    f'level:={config_data.get("level")}'])
+        config = {
+            'sim_mode': config_data.get('sim_mode'),
+            'robot': config_data.get('robot'),
+            'level': config_data.get('level'),
+        }
+        if self.control_process and self.control_process.poll() is None:
+            if config == self.control_config:
+                self.get_logger().info('Control stack is already running for this vehicle configuration.')
+                return
+            self.get_logger().warn('Vehicle configuration changed. Restarting the control stack.')
+            self.stop_control_process()
+
+        self.get_logger().info(f'Received vehicle config: {config}')
+        self.control_process = subprocess.Popen(
+            [
+                'ros2', 'launch', 'control_pkg', 'nav2_control.launch.py',
+                f'sim_mode:={config["sim_mode"]}',
+                f'robot:={config["robot"]}',
+                f'level:={config["level"]}',
+            ],
+            start_new_session=True,
+        )
+        self.control_config = config
+
+    def stop_control_process(self):
+        if not self.control_process or self.control_process.poll() is not None:
+            self.control_process = None
+            return
+
+        try:
+            os.killpg(os.getpgid(self.control_process.pid), signal.SIGTERM)
+            self.control_process.wait(timeout=5)
+        except subprocess.TimeoutExpired:
+            os.killpg(os.getpgid(self.control_process.pid), signal.SIGKILL)
+            self.control_process.wait()
+        except ProcessLookupError:
             pass
+        finally:
+            self.control_process = None
 
     def destroy_node(self):
-        if self.planning_process:
-            self.planning_process.terminate()
-            self.planning_process.wait()
-        
+        self.stop_control_process()
         return super().destroy_node()
 
 def main(args=None):
@@ -54,4 +89,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-    

src/control_pkg/launch/nav2_control.launch.py

@@ -4,7 +4,7 @@ from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.actions import GroupAction
-from launch.actions import OpaqueFunction
+from launch.actions import OpaqueFunction, TimerAction
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch.substitutions import PythonExpression
 from launch_ros.actions import Node
@@ -80,7 +80,9 @@ def launch_setup(context, *args, **kwargs):
                 parameters=[configured_params],
                 arguments=['--ros-args', '--log-level', log_level],
             ),
-            Node(
+            TimerAction(
+                period=2.0,
+                actions=[Node(
                     package='nav2_lifecycle_manager',
                     executable='lifecycle_manager',
                     name='lifecycle_manager_control',
@@ -89,6 +91,7 @@ def launch_setup(context, *args, **kwargs):
                     parameters=[{'use_sim_time': sim_mode == 'true'},
                                 {'autostart': True},
                                 {'node_names': lifecycle_nodes}]
+                )],
             ),
         ],
     )