Controller spawner timeout (backport #608) (#609)

* Simplify controller spawner definitions

* Ignore flake8 W503 as it clashes with black and goes againts PEP8 style

* Add argument to set controller spawner timeout

* Use longer controller manager timeout in CI

The default timeout of 10s is the same as our RTDE retry timeout, which
means if RTDE does not immediately connect (which happens regularly in
CI runners) controller spawning would fail.

(cherry picked from commit 5e9d442)

Co-authored-by: RobertWilbrandt <[email protected]>

Author: mergify[bot]

.pre-commit-config.yaml

@@ -54,7 +54,7 @@ repos:
     rev: 3.9.0
     hooks:
     - id: flake8
-      args: ["--ignore=E501"]
+      args: ["--ignore=E501,W503"]
 
   # CPP hooks
   # The same options as in ament_cppcheck are used, but its not working...

ur_robot_driver/launch/ur_control.launch.py

@@ -29,13 +29,13 @@
 #
 # Author: Denis Stogl
 
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.actions import OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.conditions import IfCondition, UnlessCondition
 from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
 
 
 def launch_setup(context, *args, **kwargs):
@@ -54,6 +54,7 @@ def launch_setup(context, *args, **kwargs):
     prefix = LaunchConfiguration("prefix")
     use_fake_hardware = LaunchConfiguration("use_fake_hardware")
     fake_sensor_commands = LaunchConfiguration("fake_sensor_commands")
+    controller_spawner_timeout = LaunchConfiguration("controller_spawner_timeout")
     initial_joint_controller = LaunchConfiguration("initial_joint_controller")
     activate_joint_controller = LaunchConfiguration("activate_joint_controller")
     launch_rviz = LaunchConfiguration("launch_rviz")
@@ -271,55 +272,58 @@ def launch_setup(context, *args, **kwargs):
         arguments=["-d", rviz_config_file],
     )
 
-    joint_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
-    )
+    # Spawn controllers
+    def controller_spawner(name, active=True):
+        inactive_flags = ["--inactive"] if not active else []
+        return Node(
+            package="controller_manager",
+            executable="spawner",
+            arguments=[
+                name,
+                "--controller-manager",
+                "/controller_manager",
+                "--controller-manager-timeout",
+                controller_spawner_timeout,
+            ]
+            + inactive_flags,
+        )
 
-    io_and_status_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["io_and_status_controller", "-c", "/controller_manager"],
-    )
+    controller_spawner_names = [
+        "joint_state_broadcaster",
+        "io_and_status_controller",
+        "speed_scaling_state_broadcaster",
+        "force_torque_sensor_broadcaster",
+    ]
+    controller_spawner_inactive_names = ["forward_position_controller"]
 
-    speed_scaling_state_broadcaster_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[
-            "speed_scaling_state_broadcaster",
-            "--controller-manager",
-            "/controller_manager",
-        ],
-    )
+    controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
+        controller_spawner(name, active=False) for name in controller_spawner_inactive_names
+    ]
 
-    force_torque_sensor_broadcaster_spawner = Node(
+    # 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=[
-            "force_torque_sensor_broadcaster",
-            "--controller-manager",
+            initial_joint_controller,
+            "-c",
             "/controller_manager",
+            "--controller-manager-timeout",
+            controller_spawner_timeout,
         ],
-    )
-
-    forward_position_controller_spawner_stopped = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["forward_position_controller", "-c", "/controller_manager", "--inactive"],
-    )
-
-    # 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"],
         condition=IfCondition(activate_joint_controller),
     )
     initial_joint_controller_spawner_stopped = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=[initial_joint_controller, "-c", "/controller_manager", "--inactive"],
+        arguments=[
+            initial_joint_controller,
+            "-c",
+            "/controller_manager",
+            "--controller-manager-timeout",
+            controller_spawner_timeout,
+            "--inactive",
+        ],
         condition=UnlessCondition(activate_joint_controller),
     )
 
@@ -331,14 +335,9 @@ def launch_setup(context, *args, **kwargs):
         controller_stopper_node,
         robot_state_publisher_node,
         rviz_node,
-        joint_state_broadcaster_spawner,
-        io_and_status_controller_spawner,
-        speed_scaling_state_broadcaster_spawner,
-        force_torque_sensor_broadcaster_spawner,
-        forward_position_controller_spawner_stopped,
         initial_joint_controller_spawner_stopped,
         initial_joint_controller_spawner_started,
-    ]
+    ] + controller_spawners
 
     return nodes_to_start
 
@@ -441,6 +440,13 @@ def generate_launch_description():
             description="Enable headless mode for robot control",
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "controller_spawner_timeout",
+            default_value="10",
+            description="Timeout used when spawning controllers.",
+        )
+    )
     declared_arguments.append(
         DeclareLaunchArgument(
             "initial_joint_controller",

ur_robot_driver/test/robot_driver.py

@@ -94,6 +94,7 @@ def generate_test_description():
             "robot_ip": "192.168.56.101",
             "ur_type": ur_type,
             "launch_rviz": "false",
+            "controller_spawner_timeout": str(TIMEOUT_WAIT_SERVICE_INITIAL),
             "initial_joint_controller": "scaled_joint_trajectory_controller",
             "headless_mode": "true",
             "launch_dashboard_client": "false",