Fix for Controller Switching Issue and Refactor Controller Spawning #1093

CalaW · 2024-09-06T09:53:57Z

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

Reproducing steps (use ursim)

Launch the robot driver with the initial controller set to forward_position_controller

ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5 robot_ip:=192.168.56.101 launch_rviz:=true initial_joint_controller:=forward_position_controller

In another shell, attempt to switch controllers:

ros2 control switch_controllers --deactivate forward_position_controller --activate scaled_joint_trajectory_controller

The log shows:

[ros2_control_node-1] [WARN] [1725615458.058832043] [controller_manager]: Could not 'activate' controller with name 'scaled_joint_trajectory_controller' because no controller with this name exists
[ros2_control_node-1] [WARN] [1725615458.058950707] [controller_manager]: Controller with name 'forward_position_controller' can not be deactivated since it is not active.
[ros2_control_node-1] [WARN] [1725615458.058968971] [controller_manager]: Could not deactivate controller with name 'forward_position_controller'. Check above warnings for more details. Check the state of the controllers and their required interfaces using `ros2 control list_controllers -v` CLI to get more information.

Solution

This pr ensures all four controllers are spawned in a default inactive state.
The initial_joint_controller will be activated if activate_joint_controller is true.
The number of spawners is reduced from 3 to 2, with one spawner for active controllers and another for inactive controllers.

- make all four controllers inactive - 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.

CalaW · 2024-09-06T10:04:08Z

CI failure due to #1087. I've tested with that pr reverted, and everything works well.

fmauch · 2024-09-06T10:08:09Z

CI failure due to #1087. I've tested with that pr reverted, and everything works well.

Yes, for jazzy and rolling currently only the semi-binary builds are relevant.

fmauch · 2024-09-06T10:14:55Z

ur_robot_driver/launch/ur_control.launch.py

+    if activate_joint_controller.perform(context) == "true":
+        controllers_active.append(initial_joint_controller.perform(context))
+        controllers_inactive.remove(initial_joint_controller.perform(context))


Generally I very much appreciate the simplification you suggest. Im just not particular sure about this method. From my understanding using perform(context) is kind of a hacky way around the launch system.

Wouldn't a substitution be more appropriate for this?

Also: This would effectively spawn the initial joint controller twice. Won't the second spawner at least put up a warning that this controller is already loaded/configured?

I agree that's a hacky solution. Can you please point out which substitution is in your mind that might work in this case? I'm not really familiar with every aspects of ROS 2 launch.

This will not spawn initial controller twice. Because the controller is removed from the inactive spawn list. See log below:

Log

❯ ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur5 robot_ip:=192.168.56.101 launch_rviz:=true ─╯ [INFO] [launch]: All log files can be found below /home/ortho/.ros/log/2024-09-10-17-12-17-745467-ortho-pc-274379 [INFO] [launch]: Default logging verbosity is set to INFO [INFO] [dashboard_client-2]: process started with pid [274387] [INFO] [controller_stopper_node-3]: process started with pid [274388] [INFO] [ros2_control_node-1]: process started with pid [274386] [INFO] [urscript_interface-4]: process started with pid [274389] [INFO] [robot_state_publisher-5]: process started with pid [274390] [INFO] [rviz2-6]: process started with pid [274391] [INFO] [spawner-7]: process started with pid [274455] [INFO] [spawner-8]: process started with pid [274457] [dashboard_client-2] [INFO] [1725959537.964318239] [UR_Client_Library:]: Connected: Universal Robots Dashboard Server [dashboard_client-2] [controller_stopper_node-3] [INFO] [1725959537.966518196] [Controller stopper]: Waiting for switch controller service to come up on controller_manager/switch_controller [robot_state_publisher-5] [INFO] [1725959537.978144476] [robot_state_publisher]: Robot initialized [ros2_control_node-1] [INFO] [1725959537.983871594] [controller_manager]: Subscribing to '/robot_description' topic for robot description. [ros2_control_node-1] [INFO] [1725959537.983970438] [controller_manager]: update rate is 125 Hz [ros2_control_node-1] [WARN] [1725959537.984039503] [controller_manager]: Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO RT scheduling. See [https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] for details. [ros2_control_node-1] [INFO] [1725959538.108369564] [controller_manager]: Received robot description from topic. [ros2_control_node-1] text not specified in the tf_prefix tag [ros2_control_node-1] [INFO] [1725959538.109648778] [controller_manager.resource_manager]: Loading hardware 'ur5' [ros2_control_node-1] [INFO] [1725959538.111021437] [controller_manager.resource_manager]: Loaded hardware 'ur5' from plugin 'ur_robot_driver/URPositionHardwareInterface' [ros2_control_node-1] [INFO] [1725959538.111045661] [controller_manager.resource_manager]: Initialize hardware 'ur5' [ros2_control_node-1] [INFO] [1725959538.111076852] [controller_manager.resource_manager]: Successful initialization of hardware 'ur5' [ros2_control_node-1] [INFO] [1725959538.111167948] [resource_manager]: 'configure' hardware 'ur5' [ros2_control_node-1] [INFO] [1725959538.111173506] [URPositionHardwareInterface]: Starting ...please wait... [ros2_control_node-1] [INFO] [1725959538.111180966] [URPositionHardwareInterface]: Initializing driver... [ros2_control_node-1] [WARN] [1725959538.111386570] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details. [ros2_control_node-1] [INFO] [1725959538.112588386] [UR_Client_Library:]: Negotiated RTDE protocol version to 2. [ros2_control_node-1] [INFO] [1725959538.112660696] [UR_Client_Library:]: Setting up RTDE communication with frequency 125.000000 [rviz2-6] [INFO] [1725959538.272008358] [rviz2]: Stereo is NOT SUPPORTED [rviz2-6] [INFO] [1725959538.272084495] [rviz2]: OpenGl version: 4.6 (GLSL 4.6) [rviz2-6] [INFO] [1725959538.292703263] [rviz2]: Stereo is NOT SUPPORTED [ros2_control_node-1] [INFO] [1725959539.143704987] [URPositionHardwareInterface]: Calibration checksum: 'calib_209549117540498681'. [ros2_control_node-1] [INFO] [1725959540.166923659] [URPositionHardwareInterface]: Calibration checked successfully. [ros2_control_node-1] [INFO] [1725959540.167005056] [URPositionHardwareInterface]: System successfully started! [ros2_control_node-1] [INFO] [1725959540.167020268] [resource_manager]: Successful 'configure' of hardware 'ur5' [ros2_control_node-1] [INFO] [1725959540.167062719] [resource_manager]: 'activate' hardware 'ur5' [ros2_control_node-1] [INFO] [1725959540.167075348] [URPositionHardwareInterface]: Activating HW interface [ros2_control_node-1] [INFO] [1725959540.167079188] [resource_manager]: Successful 'activate' of hardware 'ur5' [ros2_control_node-1] [INFO] [1725959540.167103766] [controller_manager]: Resource Manager has been successfully initialized. Starting Controller Manager services... [ros2_control_node-1] [WARN] [1725959540.167198111] [UR_Client_Library:]: Your system/user seems not to be setup for FIFO scheduling. We recommend using a lowlatency kernel with FIFO scheduling. See https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/blob/master/ur_robot_driver/doc/real_time.md for details. [controller_stopper_node-3] [INFO] [1725959540.181536684] [Controller stopper]: Service available [controller_stopper_node-3] [INFO] [1725959540.181616204] [Controller stopper]: Waiting for list controllers service to come up on controller_manager/list_controllers [controller_stopper_node-3] [INFO] [1725959540.181688225] [Controller stopper]: Service available [ros2_control_node-1] [INFO] [1725959540.265985347] [controller_manager]: Loading controller 'joint_trajectory_controller' [ros2_control_node-1] [INFO] [1725959540.288751495] [controller_manager]: Loading controller 'joint_state_broadcaster' [spawner-8] [INFO] [1725959540.289741112] [spawner_joint_trajectory_controller]: Loaded joint_trajectory_controller [ros2_control_node-1] [INFO] [1725959540.312958118] [controller_manager]: Configuring controller 'joint_trajectory_controller' [ros2_control_node-1] [INFO] [1725959540.313198967] [joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. [ros2_control_node-1] [INFO] [1725959540.313230910] [joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. [ros2_control_node-1] [INFO] [1725959540.313266183] [joint_trajectory_controller]: Using 'splines' interpolation method. [spawner-7] [INFO] [1725959540.315586790] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster [ros2_control_node-1] [INFO] [1725959540.322061365] [joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. [ros2_control_node-1] [INFO] [1725959540.336727308] [controller_manager]: Configuring controller 'joint_state_broadcaster' [ros2_control_node-1] [INFO] [1725959540.336781434] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published [ros2_control_node-1] [INFO] [1725959540.345631562] [controller_manager]: Loading controller 'forward_velocity_controller' [spawner-8] [INFO] [1725959540.361162216] [spawner_joint_trajectory_controller]: Loaded forward_velocity_controller [ros2_control_node-1] [INFO] [1725959540.376461870] [controller_manager]: Configuring controller 'forward_velocity_controller' [ros2_control_node-1] [INFO] [1725959540.377273696] [forward_velocity_controller]: configure successful [spawner-7] [INFO] [1725959540.377514016] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster [ros2_control_node-1] [INFO] [1725959540.386724508] [controller_manager]: Loading controller 'io_and_status_controller' [ros2_control_node-1] [INFO] [1725959540.408486666] [controller_manager]: Loading controller 'forward_position_controller' [spawner-7] [INFO] [1725959540.409447775] [spawner_joint_state_broadcaster]: Loaded io_and_status_controller [ros2_control_node-1] [INFO] [1725959540.425046210] [controller_manager]: Configuring controller 'io_and_status_controller' [spawner-8] [INFO] [1725959540.426046339] [spawner_joint_trajectory_controller]: Loaded forward_position_controller [ros2_control_node-1] [INFO] [1725959540.432845122] [controller_manager]: Configuring controller 'forward_position_controller' [ros2_control_node-1] [INFO] [1725959540.433878033] [forward_position_controller]: configure successful [spawner-7] [INFO] [1725959540.477867055] [spawner_joint_state_broadcaster]: Configured and activated io_and_status_controller [ros2_control_node-1] [INFO] [1725959540.485383246] [controller_manager]: Loading controller 'speed_scaling_state_broadcaster' [ros2_control_node-1] [INFO] [1725959540.502561561] [speed_scaling_state_broadcaster]: Loading UR SpeedScalingStateBroadcaster with tf_prefix: [spawner-7] [INFO] [1725959540.505205559] [spawner_joint_state_broadcaster]: Loaded speed_scaling_state_broadcaster [ros2_control_node-1] [INFO] [1725959540.505964259] [controller_manager]: Configuring controller 'speed_scaling_state_broadcaster' [ros2_control_node-1] [INFO] [1725959540.506010841] [speed_scaling_state_broadcaster]: Publisher rate set to : 100.0 Hz [spawner-7] [INFO] [1725959540.529272912] [spawner_joint_state_broadcaster]: Configured and activated speed_scaling_state_broadcaster [ros2_control_node-1] [INFO] [1725959540.531323177] [controller_manager]: Loading controller 'force_torque_sensor_broadcaster' [spawner-7] [INFO] [1725959540.545904343] [spawner_joint_state_broadcaster]: Loaded force_torque_sensor_broadcaster [ros2_control_node-1] [INFO] [1725959540.546974835] [controller_manager]: Configuring controller 'force_torque_sensor_broadcaster' [spawner-7] [INFO] [1725959540.572470342] [spawner_joint_state_broadcaster]: Configured and activated force_torque_sensor_broadcaster [ros2_control_node-1] [INFO] [1725959540.578299549] [controller_manager]: Loading controller 'scaled_joint_trajectory_controller' [spawner-7] [INFO] [1725959540.617789047] [spawner_joint_state_broadcaster]: Loaded scaled_joint_trajectory_controller [INFO] [spawner-8]: process has finished cleanly [pid 274457] [ros2_control_node-1] [INFO] [1725959540.618731894] [controller_manager]: Configuring controller 'scaled_joint_trajectory_controller' [ros2_control_node-1] [INFO] [1725959540.618817620] [scaled_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter. [ros2_control_node-1] [INFO] [1725959540.618826041] [scaled_joint_trajectory_controller]: Command interfaces are [position] and state interfaces are [position velocity]. [ros2_control_node-1] [INFO] [1725959540.618833064] [scaled_joint_trajectory_controller]: Using 'splines' interpolation method. [ros2_control_node-1] [INFO] [1725959540.619783663] [scaled_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz. [spawner-7] [INFO] [1725959540.641435778] [spawner_joint_state_broadcaster]: Configured and activated scaled_joint_trajectory_controller [INFO] [spawner-7]: process has finished cleanly [pid 274455] [ros2_control_node-1] [INFO] [1725959545.949381678] [UR_Client_Library:]: Robot requested program [ros2_control_node-1] [INFO] [1725959545.949432696] [UR_Client_Library:]: Sent program to robot [ros2_control_node-1] [INFO] [1725959545.991662905] [UR_Client_Library:]: Robot connected to reverse interface. Ready to receive control commands.

… separated with space

CalaW · 2024-09-10T10:10:37Z

Pushed two unsuccessful trial using substitution to achieve same functionality.

746b931 tried to construct a list of controllers as string separated by spaces. However ROS 2 launch will treat them as a single string argument, result in failure creating node. Because spawner will use the first controller name as node name, which will be "spawner_controller1 controller2 controller3".

f922559 tried to pass a list of PythonExpression substitution as argument to remove the initial joint controller from the inactive spawn list. However the substitution can only return a string, and the empty string will still be passed to the spawner. Result says the spawner tried to spawn a controller '' but failed.

fmauch · 2024-10-01T09:36:13Z

Pushed two unsuccessful trial using substitution to achieve same functionality.

746b931 tried to construct a list of controllers as string separated by spaces. However ROS 2 launch will treat them as a single string argument, result in failure creating node. Because spawner will use the first controller name as node name, which will be "spawner_controller1 controller2 controller3".

f922559 tried to pass a list of PythonExpression substitution as argument to remove the initial joint controller from the inactive spawn list. However the substitution can only return a string, and the empty string will still be passed to the spawner. Result says the spawner tried to spawn a controller '' but failed.

Thank you for trying things out. I will quickly look into that and if I don't find something working, then let's go with your initial suggestion.

Edit: I couldn't get the removal working with a substitution.

The following works, but raises a warning since the initial controller is already loaded:

    controllers_active = [
        "joint_state_broadcaster",
        "io_and_status_controller",
        "speed_scaling_state_broadcaster",
        "force_torque_sensor_broadcaster",
        initial_joint_controller,
    ]
    controllers_inactive = [
        "scaled_joint_trajectory_controller",
        "joint_trajectory_controller",
        "forward_velocity_controller",
        "forward_position_controller",
    ]

    controller_spawners = [
        controller_spawner(controllers_active),
        controller_spawner(
            controllers_inactive,
            active=False,
        ),
    ]

What I like about this is that it comes around using the opaque function.

Edit 2: Please don't see my above comment as a definite no to using an opaque function. Maybe this is the better solution.

…r named """ This reverts commit f922559.

…e string separated with space" This reverts commit 746b931.

fmauch

Let's go with that solution as this actually seems to solve the problem pretty well.

CalaW added 2 commits September 6, 2024 17:17

formatting

754f010

fmauch reviewed Sep 6, 2024

View reviewed changes

CalaW added 2 commits September 9, 2024 20:11

failure trial, because cannot pass all controllers as a single string…

746b931

… separated with space

another failure trial, the spawner wants to spawn a controller named ""

f922559

fmauch added 3 commits October 3, 2024 14:28

Revert "another failure trial, the spawner wants to spawn a controlle…

ea5f7bd

…r named """ This reverts commit f922559.

Revert "failure trial, because cannot pass all controllers as a singl…

847002a

…e string separated with space" This reverts commit 746b931.

Merge branch 'main' into main

7d4867d

fmauch approved these changes Oct 3, 2024

View reviewed changes

fmauch merged commit 61b00da into UniversalRobots:main Oct 3, 2024
11 checks passed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

CalaW commented Sep 6, 2024

CalaW commented Sep 6, 2024

fmauch commented Sep 6, 2024

fmauch Sep 6, 2024

CalaW Sep 10, 2024

CalaW commented Sep 10, 2024

fmauch commented Oct 1, 2024 •

edited

Loading

fmauch left a comment

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

Conversation

CalaW commented Sep 6, 2024

Reproducing steps (use ursim)

Solution

CalaW commented Sep 6, 2024

fmauch commented Sep 6, 2024

fmauch Sep 6, 2024

Choose a reason for hiding this comment

CalaW Sep 10, 2024

Choose a reason for hiding this comment

CalaW commented Sep 10, 2024

fmauch commented Oct 1, 2024 • edited Loading

fmauch left a comment

Choose a reason for hiding this comment

fmauch commented Oct 1, 2024 •

edited

Loading