From 8cbf47cb5a1c2ee9c2c41ddc822a74524455e678 Mon Sep 17 00:00:00 2001 From: Chen Chen Date: Fri, 6 Sep 2024 17:17:21 +0800 Subject: [PATCH 1/6] Refactor controller spawning - 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. --- ur_robot_driver/launch/ur_control.launch.py | 56 ++++++++------------- 1 file changed, 21 insertions(+), 35 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c884086fa..ec02d45f8 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -34,7 +34,7 @@ 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.substitutions import ( AndSubstitution, @@ -45,7 +45,7 @@ from launch.launch_description_sources import AnyLaunchDescriptionSource -def launch_setup(): +def launch_setup(context): # Initialize Arguments ur_type = LaunchConfiguration("ur_type") robot_ip = LaunchConfiguration("robot_ip") @@ -163,38 +163,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 +194,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 +308,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 +465,4 @@ def generate_launch_description(): ], ) ) - return LaunchDescription(declared_arguments + launch_setup()) + return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)]) From 754f010d3958c7631a47a4a90c92b3119b6fa4d9 Mon Sep 17 00:00:00 2001 From: Chen Chen Date: Fri, 6 Sep 2024 17:22:12 +0800 Subject: [PATCH 2/6] formatting --- ur_robot_driver/launch/ur_control.launch.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index ec02d45f8..fd456e2e3 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -29,20 +29,23 @@ # # 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, OpaqueFunction +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(context): From 746b9314534b324b63b1a06b38e731d573993524 Mon Sep 17 00:00:00 2001 From: Chen Chen Date: Mon, 9 Sep 2024 20:11:34 +0800 Subject: [PATCH 3/6] failure trial, because cannot pass all controllers as a single string separated with space --- ur_robot_driver/launch/ur_control.launch.py | 52 +++++++++++++++++---- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index fd456e2e3..c33344a7c 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -42,6 +42,7 @@ LaunchConfiguration, NotSubstitution, PathJoinSubstitution, + PythonExpression, ) from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterFile @@ -145,7 +146,7 @@ def launch_setup(context): ) # Spawn controllers - def controller_spawner(controllers, active=True): + def controller_spawner(controllers, condition, active=True): inactive_flags = ["--inactive"] if not active else [] return Node( package="controller_manager", @@ -157,28 +158,61 @@ def controller_spawner(controllers, active=True): controller_spawner_timeout, ] + inactive_flags - + controllers, + + [controllers], + condition=condition, ) - controllers_active = [ + controllers_active_default = [ "joint_state_broadcaster", "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controllers_inactive = [ + controllers_inactive_default = [ "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)) + controllers_active_init = PythonExpression( + [ + "'", + " ".join(controllers_active_default), + "' + ' ' +'", + initial_joint_controller, + "'", + ] + ) + controllers_inactive_init = PythonExpression( + [ + "' '.join([c for c in ", + str(controllers_inactive_default), + "if c != '", + initial_joint_controller, + "'])", + ] + ) + print(controllers_active_init.perform(context)) + print(controllers_inactive_init.perform(context)) controller_spawners = [ - controller_spawner(controllers_active), - controller_spawner(controllers_inactive, active=False), + controller_spawner( + controllers_active_init, condition=IfCondition(activate_joint_controller) + ), + controller_spawner( + controllers_inactive_init, + condition=IfCondition(activate_joint_controller), + active=False, + ), + controller_spawner( + " ".join(controllers_active_default), + condition=UnlessCondition(activate_joint_controller), + ), + controller_spawner( + " ".join(controllers_inactive_default), + condition=UnlessCondition(activate_joint_controller), + active=False, + ), ] rsp = IncludeLaunchDescription( From f92255929ce124c389e14b6d2bc0e9d46ddcb8f9 Mon Sep 17 00:00:00 2001 From: Chen Chen Date: Mon, 9 Sep 2024 21:38:32 +0800 Subject: [PATCH 4/6] another failure trial, the spawner wants to spawn a controller named "" --- ur_robot_driver/launch/ur_control.launch.py | 31 ++++++--------------- 1 file changed, 8 insertions(+), 23 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c33344a7c..0382b0484 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -158,7 +158,7 @@ def controller_spawner(controllers, condition, active=True): controller_spawner_timeout, ] + inactive_flags - + [controllers], + + controllers, condition=condition, ) @@ -174,26 +174,11 @@ def controller_spawner(controllers, condition, active=True): "forward_velocity_controller", "forward_position_controller", ] - controllers_active_init = PythonExpression( - [ - "'", - " ".join(controllers_active_default), - "' + ' ' +'", - initial_joint_controller, - "'", - ] - ) - controllers_inactive_init = PythonExpression( - [ - "' '.join([c for c in ", - str(controllers_inactive_default), - "if c != '", - initial_joint_controller, - "'])", - ] - ) - print(controllers_active_init.perform(context)) - print(controllers_inactive_init.perform(context)) + controllers_active_init = [*controllers_active_default, initial_joint_controller] + controllers_inactive_init = [ + PythonExpression(["'", c, "' if '", c, "' != '", initial_joint_controller, "' else ''"]) + for c in controllers_inactive_default + ] controller_spawners = [ controller_spawner( @@ -205,11 +190,11 @@ def controller_spawner(controllers, condition, active=True): active=False, ), controller_spawner( - " ".join(controllers_active_default), + controllers_active_default, condition=UnlessCondition(activate_joint_controller), ), controller_spawner( - " ".join(controllers_inactive_default), + controllers_inactive_default, condition=UnlessCondition(activate_joint_controller), active=False, ), From ea5f7bd209ac4e0192c185367f8825ce3af558e7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 14:28:50 +0200 Subject: [PATCH 5/6] Revert "another failure trial, the spawner wants to spawn a controller named """ This reverts commit f92255929ce124c389e14b6d2bc0e9d46ddcb8f9. --- ur_robot_driver/launch/ur_control.launch.py | 31 +++++++++++++++------ 1 file changed, 23 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 0382b0484..c33344a7c 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -158,7 +158,7 @@ def controller_spawner(controllers, condition, active=True): controller_spawner_timeout, ] + inactive_flags - + controllers, + + [controllers], condition=condition, ) @@ -174,11 +174,26 @@ def controller_spawner(controllers, condition, active=True): "forward_velocity_controller", "forward_position_controller", ] - controllers_active_init = [*controllers_active_default, initial_joint_controller] - controllers_inactive_init = [ - PythonExpression(["'", c, "' if '", c, "' != '", initial_joint_controller, "' else ''"]) - for c in controllers_inactive_default - ] + controllers_active_init = PythonExpression( + [ + "'", + " ".join(controllers_active_default), + "' + ' ' +'", + initial_joint_controller, + "'", + ] + ) + controllers_inactive_init = PythonExpression( + [ + "' '.join([c for c in ", + str(controllers_inactive_default), + "if c != '", + initial_joint_controller, + "'])", + ] + ) + print(controllers_active_init.perform(context)) + print(controllers_inactive_init.perform(context)) controller_spawners = [ controller_spawner( @@ -190,11 +205,11 @@ def controller_spawner(controllers, condition, active=True): active=False, ), controller_spawner( - controllers_active_default, + " ".join(controllers_active_default), condition=UnlessCondition(activate_joint_controller), ), controller_spawner( - controllers_inactive_default, + " ".join(controllers_inactive_default), condition=UnlessCondition(activate_joint_controller), active=False, ), From 847002a42b33b0a0c783eff6df25ccdd5ccc899d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 14:28:56 +0200 Subject: [PATCH 6/6] Revert "failure trial, because cannot pass all controllers as a single string separated with space" This reverts commit 746b9314534b324b63b1a06b38e731d573993524. --- ur_robot_driver/launch/ur_control.launch.py | 52 ++++----------------- 1 file changed, 9 insertions(+), 43 deletions(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index c33344a7c..fd456e2e3 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -42,7 +42,6 @@ LaunchConfiguration, NotSubstitution, PathJoinSubstitution, - PythonExpression, ) from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterFile @@ -146,7 +145,7 @@ def launch_setup(context): ) # Spawn controllers - def controller_spawner(controllers, condition, active=True): + def controller_spawner(controllers, active=True): inactive_flags = ["--inactive"] if not active else [] return Node( package="controller_manager", @@ -158,61 +157,28 @@ def controller_spawner(controllers, condition, active=True): controller_spawner_timeout, ] + inactive_flags - + [controllers], - condition=condition, + + controllers, ) - controllers_active_default = [ + controllers_active = [ "joint_state_broadcaster", "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", ] - controllers_inactive_default = [ + controllers_inactive = [ "scaled_joint_trajectory_controller", "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", ] - controllers_active_init = PythonExpression( - [ - "'", - " ".join(controllers_active_default), - "' + ' ' +'", - initial_joint_controller, - "'", - ] - ) - controllers_inactive_init = PythonExpression( - [ - "' '.join([c for c in ", - str(controllers_inactive_default), - "if c != '", - initial_joint_controller, - "'])", - ] - ) - print(controllers_active_init.perform(context)) - print(controllers_inactive_init.perform(context)) + if activate_joint_controller.perform(context) == "true": + controllers_active.append(initial_joint_controller.perform(context)) + controllers_inactive.remove(initial_joint_controller.perform(context)) controller_spawners = [ - controller_spawner( - controllers_active_init, condition=IfCondition(activate_joint_controller) - ), - controller_spawner( - controllers_inactive_init, - condition=IfCondition(activate_joint_controller), - active=False, - ), - controller_spawner( - " ".join(controllers_active_default), - condition=UnlessCondition(activate_joint_controller), - ), - controller_spawner( - " ".join(controllers_inactive_default), - condition=UnlessCondition(activate_joint_controller), - active=False, - ), + controller_spawner(controllers_active), + controller_spawner(controllers_inactive, active=False), ] rsp = IncludeLaunchDescription(