From 30bad12443325a33cb98350d2582dae6b1919c51 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 23 Aug 2024 22:07:00 +0000 Subject: [PATCH] Use spawner instead of cli verbs --- .../config/ackermann_drive_controller.yaml | 5 +- .../config/cart_controller_effort.yaml | 4 +- .../config/cart_controller_position.yaml | 4 +- .../config/cart_controller_velocity.yaml | 7 +-- .../diff_drive_controller_velocity.yaml | 4 +- .../config/gripper_controller_effort.yaml | 4 +- .../config/gripper_controller_position.yaml | 4 +- .../config/tricycle_drive_controller.yaml | 9 ++-- .../launch/ackermann_drive_example.launch.py | 35 ++++++++------ .../launch/cart_example_effort.launch.py | 36 +++++++++------ .../launch/cart_example_position.launch.py | 36 +++++++++------ .../launch/cart_example_velocity.launch.py | 46 +++++++++++++------ .../launch/diff_drive_example.launch.py | 36 +++++++++------ ...ipper_mimic_joint_example_effort.launch.py | 36 +++++++++------ ...per_mimic_joint_example_position.launch.py | 36 +++++++++------ .../launch/pendulum_example_effort.launch.py | 36 +++++++++------ .../pendulum_example_position.launch.py | 36 +++++++++------ .../launch/tricycle_drive_example.launch.py | 40 ++++++++++------ .../config/cart_controller_position.yaml | 4 +- gz_ros2_control_tests/tests/position_test.py | 44 +++++++++++------- 20 files changed, 282 insertions(+), 180 deletions(-) diff --git a/gz_ros2_control_demos/config/ackermann_drive_controller.yaml b/gz_ros2_control_demos/config/ackermann_drive_controller.yaml index e834c438..a3651db8 100644 --- a/gz_ros2_control_demos/config/ackermann_drive_controller.yaml +++ b/gz_ros2_control_demos/config/ackermann_drive_controller.yaml @@ -5,12 +5,9 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - - ackermann_steering_controller: - type: 'ackermann_steering_controller/AckermannSteeringController' - ackermann_steering_controller: ros__parameters: + type: 'ackermann_steering_controller/AckermannSteeringController' wheelbase: 1.7 front_wheel_track: 1.0 rear_wheel_track: 1.0 diff --git a/gz_ros2_control_demos/config/cart_controller_effort.yaml b/gz_ros2_control_demos/config/cart_controller_effort.yaml index 920e4651..9f72d0c1 100644 --- a/gz_ros2_control_demos/config/cart_controller_effort.yaml +++ b/gz_ros2_control_demos/config/cart_controller_effort.yaml @@ -2,13 +2,11 @@ controller_manager: ros__parameters: update_rate: 1000 # Hz - effort_controller: - type: effort_controllers/JointGroupEffortController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster effort_controller: ros__parameters: + type: effort_controllers/JointGroupEffortController joints: - slider_to_cart diff --git a/gz_ros2_control_demos/config/cart_controller_position.yaml b/gz_ros2_control_demos/config/cart_controller_position.yaml index 835a8360..543a99c6 100644 --- a/gz_ros2_control_demos/config/cart_controller_position.yaml +++ b/gz_ros2_control_demos/config/cart_controller_position.yaml @@ -2,14 +2,12 @@ controller_manager: ros__parameters: update_rate: 1000 # Hz - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController joints: - slider_to_cart command_interfaces: diff --git a/gz_ros2_control_demos/config/cart_controller_velocity.yaml b/gz_ros2_control_demos/config/cart_controller_velocity.yaml index 7215d760..bc3f51cd 100644 --- a/gz_ros2_control_demos/config/cart_controller_velocity.yaml +++ b/gz_ros2_control_demos/config/cart_controller_velocity.yaml @@ -2,15 +2,9 @@ controller_manager: ros__parameters: update_rate: 1000 # Hz - velocity_controller: - type: velocity_controllers/JointGroupVelocityController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - imu_sensor_broadcaster: - type: imu_sensor_broadcaster/IMUSensorBroadcaster - velocity_controller: ros__parameters: joints: @@ -18,5 +12,6 @@ velocity_controller: imu_sensor_broadcaster: ros__parameters: + type: imu_sensor_broadcaster/IMUSensorBroadcaster sensor_name: cart_imu_sensor frame_id: imu diff --git a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml index a5c426d2..6335341f 100644 --- a/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml +++ b/gz_ros2_control_demos/config/diff_drive_controller_velocity.yaml @@ -5,11 +5,9 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster - diff_drive_base_controller: - type: diff_drive_controller/DiffDriveController - diff_drive_base_controller: ros__parameters: + type: diff_drive_controller/DiffDriveController left_wheel_names: ["left_wheel_joint"] right_wheel_names: ["right_wheel_joint"] diff --git a/gz_ros2_control_demos/config/gripper_controller_effort.yaml b/gz_ros2_control_demos/config/gripper_controller_effort.yaml index ac5855eb..2440bca8 100644 --- a/gz_ros2_control_demos/config/gripper_controller_effort.yaml +++ b/gz_ros2_control_demos/config/gripper_controller_effort.yaml @@ -2,14 +2,12 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - gripper_controller: - type: forward_command_controller/ForwardCommandController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster gripper_controller: ros__parameters: + type: forward_command_controller/ForwardCommandController joints: - right_finger_joint interface_name: effort diff --git a/gz_ros2_control_demos/config/gripper_controller_position.yaml b/gz_ros2_control_demos/config/gripper_controller_position.yaml index d72296e1..e8bedb36 100644 --- a/gz_ros2_control_demos/config/gripper_controller_position.yaml +++ b/gz_ros2_control_demos/config/gripper_controller_position.yaml @@ -2,14 +2,12 @@ controller_manager: ros__parameters: update_rate: 100 # Hz - gripper_controller: - type: forward_command_controller/ForwardCommandController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster gripper_controller: ros__parameters: + type: forward_command_controller/ForwardCommandController joints: - right_finger_joint interface_name: position diff --git a/gz_ros2_control_demos/config/tricycle_drive_controller.yaml b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml index c8e0c5b7..ff90ad57 100644 --- a/gz_ros2_control_demos/config/tricycle_drive_controller.yaml +++ b/gz_ros2_control_demos/config/tricycle_drive_controller.yaml @@ -2,18 +2,15 @@ controller_manager: ros__parameters: update_rate: 50 # Hz - tricycle_controller: - type: tricycle_controller/TricycleController - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - joint_state_broadcaster: ros__parameters: + type: joint_state_broadcaster/JointStateBroadcaster extra_joints: ["right_wheel_joint", "left_wheel_joint"] tricycle_controller: ros__parameters: + type: tricycle_controller/TricycleController + # Model traction_joint_name: traction_joint # Name of traction joint in URDF steering_joint_name: steering_joint # Name of steering joint in URDF diff --git a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py index cb2bbf99..fb6f065b 100644 --- a/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/ackermann_drive_example.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'ackermann_drive_controller.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,18 @@ def generate_launch_description(): 'ackermann', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_ackermann_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'ackermann_steering_controller'], - output='screen' + ackermann_steering_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['ackermann_steering_controller', + '--param-file', + robot_controllers, + ], ) # Bridge @@ -87,13 +96,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_ackermann_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[ackermann_steering_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/cart_example_effort.launch.py b/gz_ros2_control_demos/launch/cart_example_effort.launch.py index 8d6b4fe9..211e1c95 100644 --- a/gz_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_effort.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_effort.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): '-name', 'cart', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_effort_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', - '--set-state', 'active', 'effort_controller'], - output='screen' + effort_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'effort_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +88,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_effort_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[effort_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/cart_example_position.launch.py b/gz_ros2_control_demos/launch/cart_example_position.launch.py index 14391bb4..17658998 100644 --- a/gz_ros2_control_demos/launch/cart_example_position.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_position.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_position.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): '-name', 'cart', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +88,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/cart_example_velocity.launch.py b/gz_ros2_control_demos/launch/cart_example_velocity.launch.py index 3406e35d..ee4786ff 100644 --- a/gz_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/gz_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_velocity.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,28 @@ def generate_launch_description(): '-name', 'cart', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_velocity_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', - '--set-state', 'active', 'velocity_controller'], - output='screen' + velocity_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'velocity_controller', + '--param-file', + robot_controllers, + ], + ) + imu_sensor_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'imu_sensor_broadcaster', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +97,14 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_velocity_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[velocity_controller_spawner, + imu_sensor_broadcaster_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/diff_drive_example.launch.py b/gz_ros2_control_demos/launch/diff_drive_example.launch.py index f91560d3..44b23a78 100644 --- a/gz_ros2_control_demos/launch/diff_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/diff_drive_example.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'diff_drive_controller_velocity.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): 'diff_drive', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_diff_drive_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'diff_drive_base_controller'], - output='screen' + diff_drive_base_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'diff_drive_base_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +88,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_diff_drive_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[diff_drive_base_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py index dbeae8db..e4f39eb6 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.py @@ -16,7 +16,7 @@ # from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -42,6 +42,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'gripper_controller_effort.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -58,16 +65,19 @@ def generate_launch_description(): 'gripper', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_gripper_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'gripper_controller'], - output='screen' + gripper_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'gripper_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -81,13 +91,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_gripper_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[gripper_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py index 6e932da5..fe936755 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.py @@ -16,7 +16,7 @@ # from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -42,6 +42,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'gripper_controller_position.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -58,16 +65,19 @@ def generate_launch_description(): 'gripper', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_gripper_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'gripper_controller'], - output='screen' + gripper_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'gripper_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -81,13 +91,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_gripper_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[gripper_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py b/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py index d09ae2f9..e8b25df8 100644 --- a/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py +++ b/gz_ros2_control_demos/launch/pendulum_example_effort.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_effort.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): '-name', 'cart', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_effort_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', - '--set-state', 'active', 'effort_controller'], - output='screen' + effort_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'effort_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +88,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_effort_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[effort_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/pendulum_example_position.launch.py b/gz_ros2_control_demos/launch/pendulum_example_position.launch.py index 24045420..2a920394 100644 --- a/gz_ros2_control_demos/launch/pendulum_example_position.launch.py +++ b/gz_ros2_control_demos/launch/pendulum_example_position.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'cart_controller_position.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,19 @@ def generate_launch_description(): '-name', 'cart', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_controller', + '--param-file', + robot_controllers, + ], ) return LaunchDescription([ @@ -78,13 +88,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py b/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py index 9b2d31b6..a1ab9c20 100644 --- a/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py +++ b/gz_ros2_control_demos/launch/tricycle_drive_example.launch.py @@ -13,7 +13,7 @@ # limitations under the License. from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -39,6 +39,13 @@ def generate_launch_description(): ] ) robot_description = {'robot_description': robot_description_content} + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare('gz_ros2_control_demos'), + 'config', + 'tricycle_drive_controller.yaml', + ] + ) node_robot_state_publisher = Node( package='robot_state_publisher', @@ -55,16 +62,23 @@ def generate_launch_description(): 'tricyle', '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_state_broadcaster', + '--param-file', + robot_controllers, + ], ) - - load_tricycle_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'tricycle_controller'], - output='screen' + tricycle_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'tricycle_controller', + '--param-file', + robot_controllers, + ], ) # Bridge @@ -87,13 +101,13 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_tricycle_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[tricycle_controller_spawner], ) ), node_robot_state_publisher, diff --git a/gz_ros2_control_tests/config/cart_controller_position.yaml b/gz_ros2_control_tests/config/cart_controller_position.yaml index 835a8360..543a99c6 100644 --- a/gz_ros2_control_tests/config/cart_controller_position.yaml +++ b/gz_ros2_control_tests/config/cart_controller_position.yaml @@ -2,14 +2,12 @@ controller_manager: ros__parameters: update_rate: 1000 # Hz - joint_trajectory_controller: - type: joint_trajectory_controller/JointTrajectoryController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController joints: - slider_to_cart command_interfaces: diff --git a/gz_ros2_control_tests/tests/position_test.py b/gz_ros2_control_tests/tests/position_test.py index 7080c719..ddb775cc 100755 --- a/gz_ros2_control_tests/tests/position_test.py +++ b/gz_ros2_control_tests/tests/position_test.py @@ -19,7 +19,7 @@ from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import ExecuteProcess, IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -59,6 +59,11 @@ def generate_test_description(): xacro_file = os.path.join(gz_ros2_control_tests_path, 'urdf', 'test_cart_position.xacro.urdf') + robot_controllers = os.path.join( + gz_ros2_control_tests_path, + 'config', + 'cart_controller_position.yaml' + ) print('xacro_file ', xacro_file) doc = xacro.parse(open(xacro_file)) @@ -81,16 +86,19 @@ def generate_test_description(): '-allow_renaming', 'true'], ) - load_joint_state_broadcaster = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], - output='screen' + joint_state_broadcaster_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=['joint_state_broadcaster'], ) - - load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_trajectory_controller'], - output='screen' + joint_trajectory_controller_spawner = Node( + package='controller_manager', + executable='spawner', + arguments=[ + 'joint_trajectory_controller', + '--param-file', + robot_controllers, + ], ) ld = launch.LaunchDescription([ @@ -100,13 +108,13 @@ def generate_test_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=gz_spawn_entity, - on_exit=[load_joint_state_broadcaster], + on_exit=[joint_state_broadcaster_spawner], ) ), RegisterEventHandler( event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_joint_trajectory_controller], + target_action=joint_state_broadcaster_spawner, + on_exit=[joint_trajectory_controller_spawner], ) ), KeepAliveProc(), @@ -120,9 +128,10 @@ def generate_test_description(): class TestFixture(unittest.TestCase): def test_arm(self, launch_service, proc_info, proc_output): - proc_output.assertWaitFor('Successfully loaded controller joint_trajectory_controller ' - 'into state active', - timeout=100, stream='stdout') + proc_output.assertWaitFor('Configured and activated', + process='spawner', + cmd_args=['joint_trajectory_controller'], + timeout=100) proc_action = Node( package='gz_ros2_control_tests', @@ -137,7 +146,10 @@ def test_arm(self, launch_service, proc_info, proc_output): launch_testing.asserts.assertExitCodes(proc_info, process=proc_action, allowable_exit_codes=[0]) + def tearDown(self): for proc in psutil.process_iter(): # check whether the process name matches if proc.name() == 'ruby': proc.kill() + if 'gz sim' in proc.name(): + proc.kill()