Skip to content

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 7 commits into from
Oct 3, 2024
+29 −40
Merged

Fix for Controller Switching Issue and Refactor Controller Spawning #1093

Changes from 2 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8cbf47c
Refactor controller spawning
CalaW Sep 6, 2024
754f010
formatting
CalaW Sep 6, 2024
746b931
failure trial, because cannot pass all controllers as a single string…
CalaW Sep 9, 2024
f922559
another failure trial, the spawner wants to spawn a controller named ""
CalaW Sep 9, 2024
ea5f7bd
Revert "another failure trial, the spawner wants to spawn a controlle…
Oct 3, 2024
847002a
Revert "failure trial, because cannot pass all controllers as a singl…
Oct 3, 2024
7d4867d
Merge branch 'main' into main
Oct 3, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
69 changes: 29 additions & 40 deletions ur_robot_driver/launch/ur_control.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down Expand Up @@ -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))
Comment on lines +175 to +177
Copy link
Contributor

@fmauch fmauch Sep 6, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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?

Copy link
Contributor Author

@CalaW CalaW Sep 10, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


# 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),
Expand All @@ -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
Expand Down Expand Up @@ -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.",
)
)
Expand Down Expand Up @@ -479,4 +468,4 @@ def generate_launch_description():
],
)
)
return LaunchDescription(declared_arguments + launch_setup())
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])