Fix for Controller Switching Issue and Refactor Controller Spawning (#1093)

- make all controllers inactive initially
- active the initial_joint_controller if needed
- reduce the number of spawners from 3 to 2 (active controllers and inactive controllers)

Fixes the issue when forward_position_controller is the initial controller, the user can't switch to scaled_joint_trajectory_controller because it's never spawned.

Author: CalaW

ur_robot_driver/launch/ur_control.launch.py

@@ -29,23 +29,26 @@
 #
 # Author: Denis Stogl
 
-from launch_ros.actions import Node
-from launch_ros.parameter_descriptions import ParameterFile
-from launch_ros.substitutions import FindPackageShare
-
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    IncludeLaunchDescription,
+    OpaqueFunction,
+)
 from launch.conditions import IfCondition, UnlessCondition
+from launch.launch_description_sources import AnyLaunchDescriptionSource
 from launch.substitutions import (
     AndSubstitution,
     LaunchConfiguration,
     NotSubstitution,
     PathJoinSubstitution,
 )
-from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterFile
+from launch_ros.substitutions import FindPackageShare
 
 
-def launch_setup():
+def launch_setup(context):
     # Initialize Arguments
     ur_type = LaunchConfiguration("ur_type")
     robot_ip = LaunchConfiguration("robot_ip")
@@ -163,38 +166,20 @@ def controller_spawner(controllers, active=True):
         "speed_scaling_state_broadcaster",
         "force_torque_sensor_broadcaster",
     ]
-    controllers_inactive = ["forward_position_controller"]
-
-    controller_spawners = [controller_spawner(controllers_active)] + [
-        controller_spawner(controllers_inactive, active=False)
+    controllers_inactive = [
+        "scaled_joint_trajectory_controller",
+        "joint_trajectory_controller",
+        "forward_velocity_controller",
+        "forward_position_controller",
     ]
+    if activate_joint_controller.perform(context) == "true":
+        controllers_active.append(initial_joint_controller.perform(context))
+        controllers_inactive.remove(initial_joint_controller.perform(context))
 
-    # There may be other controllers of the joints, but this is the initially-started one
-    initial_joint_controller_spawner_started = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-        ],
-        condition=IfCondition(activate_joint_controller),
-    )
-    initial_joint_controller_spawner_stopped = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            initial_joint_controller,
-            "-c",
-            "/controller_manager",
-            "--controller-manager-timeout",
-            controller_spawner_timeout,
-            "--inactive",
-        ],
-        condition=UnlessCondition(activate_joint_controller),
-    )
+    controller_spawners = [
+        controller_spawner(controllers_active),
+        controller_spawner(controllers_inactive, active=False),
+    ]
 
     rsp = IncludeLaunchDescription(
         AnyLaunchDescriptionSource(description_launchfile),
@@ -212,8 +197,6 @@ def controller_spawner(controllers, active=True):
         urscript_interface,
         rsp,
         rviz_node,
-        initial_joint_controller_spawner_stopped,
-        initial_joint_controller_spawner_started,
     ] + controller_spawners
 
     return nodes_to_start
@@ -328,6 +311,12 @@ def generate_launch_description():
         DeclareLaunchArgument(
             "initial_joint_controller",
             default_value="scaled_joint_trajectory_controller",
+            choices=[
+                "scaled_joint_trajectory_controller",
+                "joint_trajectory_controller",
+                "forward_velocity_controller",
+                "forward_position_controller",
+            ],
             description="Initially loaded robot controller.",
         )
     )
@@ -479,4 +468,4 @@ def generate_launch_description():
             ],
         )
     )
-    return LaunchDescription(declared_arguments + launch_setup())
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])