update controller spawning

kroshu · Oct 2, 2024 · 9a063a5 · 9a063a5
1 parent e87c631
commit 9a063a5
Show file tree

Hide file tree

Showing 3 changed files with 51 additions and 33 deletions.
diff --git a/kuka_iiqka_eac_driver/launch/startup.launch.py b/kuka_iiqka_eac_driver/launch/startup.launch.py
@@ -145,28 +145,34 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Spawn controllers
-    def controller_spawner(controller_names, activate=False):
+    def controller_spawner(controller_name, param_file=None, activate=False):
         arg_list = [
-            controller_names,
+            controller_name,
             "-c",
             controller_manager_node,
             "-n",
             ns,
         ]
+
+        # Add param-file if it's provided
+        if param_file:
+            arg_list.extend(["--param-file", param_file])
+
         if not activate:
             arg_list.append("--inactive")
+
         return Node(package="controller_manager", executable="spawner", arguments=arg_list)
 
-    controller_names = [
-        "joint_state_broadcaster",
-        "joint_trajectory_controller",
-        "joint_group_impedance_controller",
-        "effort_controller",
-        "control_mode_handler",
-        "event_broadcaster",
-    ]
+    controllers = {
+        "joint_state_broadcaster": None,
+        "joint_trajectory_controller": jtc_config,
+        "joint_group_impedance_controller": jic_config,
+        "effort_controller": ec_config,
+        "control_mode_handler": None,
+        "event_broadcaster": None,
+    }
 
-    controller_spawners = [controller_spawner(name) for name in controller_names]
+    controller_spawners = [controller_spawner(name, param_file) for name, param_file in controllers.items()]
 
     nodes_to_start = [
         control_node,

diff --git a/kuka_kss_rsi_driver/launch/startup.launch.py b/kuka_kss_rsi_driver/launch/startup.launch.py
@@ -128,24 +128,30 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Spawn controllers
-    def controller_spawner(controller_names, activate=False):
+    def controller_spawner(controller_name, param_file=None, activate=False):
         arg_list = [
-            controller_names,
+            controller_name,
             "-c",
             controller_manager_node,
             "-n",
             ns,
         ]
+
+        # Add param-file if it's provided
+        if param_file:
+            arg_list.extend(["--param-file", param_file])
+
         if not activate:
             arg_list.append("--inactive")
+
         return Node(package="controller_manager", executable="spawner", arguments=arg_list)
 
-    controller_names = [
-        "joint_state_broadcaster",
-        "joint_trajectory_controller",
-    ]
+    controllers = {
+        "joint_state_broadcaster": None,
+        "joint_trajectory_controller": jtc_config,
+    }
 
-    controller_spawners = [controller_spawner(name) for name in controller_names]
+    controller_spawners = [controller_spawner(name, param_file) for name, param_file in controllers.items()]
 
     nodes_to_start = [
         control_node,

diff --git a/kuka_sunrise_fri_driver/launch/startup.launch.py b/kuka_sunrise_fri_driver/launch/startup.launch.py
@@ -147,31 +147,37 @@ def launch_setup(context, *args, **kwargs):
     )
 
     # Spawn controllers
-    def controller_spawner(controller_names, activate=False):
+    def controller_spawner(controller_name, param_file=None, activate=False):
         arg_list = [
-            controller_names,
+            controller_name,
             "-c",
             controller_manager_node,
             "-n",
             ns,
         ]
+
+        # Add param-file if it's provided
+        if param_file:
+            arg_list.extend(["--param-file", param_file])
+
         if not activate:
             arg_list.append("--inactive")
+
         return Node(package="controller_manager", executable="spawner", arguments=arg_list)
 
-    controller_names = [
-        "joint_state_broadcaster",
-        "external_torque_broadcaster",
-        "joint_trajectory_controller",
-        "fri_configuration_controller",
-        "fri_state_broadcaster",
-        "joint_group_impedance_controller",
-        "effort_controller",
-        "control_mode_handler",
-        "event_broadcaster",
-    ]
-
-    controller_spawners = [controller_spawner(name) for name in controller_names]
+    controllers = {
+        "joint_state_broadcaster": None,
+        "external_torque_broadcaster": etb_config,
+        "joint_trajectory_controller": jtc_config,
+        "fri_configuration_controller": None,
+        "fri_state_broadcaster": None,
+        "joint_group_impedance_controller": jic_config,
+        "effort_controller": ec_config,
+        "control_mode_handler": None,
+        "event_broadcaster": None,
+    }
+
+    controller_spawners = [controller_spawner(name, param_file) for name, param_file in controllers.items()]
 
     nodes_to_start = [
         control_node,