From 30bad12443325a33cb98350d2582dae6b1919c51 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 23 Aug 2024 22:07:00 +0000 Subject: [PATCH 1/9] 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() From c1debc1a8b4fd7e18553d8671f5a45261d2c87e1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 09:55:39 +0000 Subject: [PATCH 2/9] Fix velocity controller --- gz_ros2_control_demos/config/cart_controller_velocity.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/gz_ros2_control_demos/config/cart_controller_velocity.yaml b/gz_ros2_control_demos/config/cart_controller_velocity.yaml index bc3f51cd..f215830f 100644 --- a/gz_ros2_control_demos/config/cart_controller_velocity.yaml +++ b/gz_ros2_control_demos/config/cart_controller_velocity.yaml @@ -7,6 +7,7 @@ controller_manager: velocity_controller: ros__parameters: + type: velocity_controllers/JointGroupVelocityController joints: - slider_to_cart From 35a16efb484e7f4de58445ae24a4edbe7fa37ac1 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 09:56:20 +0000 Subject: [PATCH 3/9] Fix ackermann twist reference --- .../examples/example_ackermann_drive.cpp | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp index 7e3b4414..35469e25 100644 --- a/gz_ros2_control_demos/examples/example_ackermann_drive.cpp +++ b/gz_ros2_control_demos/examples/example_ackermann_drive.cpp @@ -17,7 +17,7 @@ #include -#include +#include using namespace std::chrono_literals; @@ -28,22 +28,26 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("ackermann_drive_test_node"); - auto publisher = node->create_publisher( - "/ackermann_steering_controller/reference_unstamped", 10); + auto publisher = node->create_publisher( + "/ackermann_steering_controller/reference", 10); RCLCPP_INFO(node->get_logger(), "node created"); - geometry_msgs::msg::Twist command; + geometry_msgs::msg::Twist tw; - command.linear.x = 0.5; - command.linear.y = 0.0; - command.linear.z = 0.0; + tw.linear.x = 0.5; + tw.linear.y = 0.0; + tw.linear.z = 0.0; - command.angular.x = 0.0; - command.angular.y = 0.0; - command.angular.z = 0.3; + tw.angular.x = 0.0; + tw.angular.y = 0.0; + tw.angular.z = 0.3; + + geometry_msgs::msg::TwistStamped command; + command.twist = tw; while (1) { + command.header.stamp = node->now(); publisher->publish(command); std::this_thread::sleep_for(50ms); rclcpp::spin_some(node); From 4cd3298334a5105c5e143a23e07fae2adc19e12f Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 09:56:30 +0000 Subject: [PATCH 4/9] Sort dependencies --- gz_ros2_control_demos/package.xml | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/gz_ros2_control_demos/package.xml b/gz_ros2_control_demos/package.xml index c9ac1ee8..f63ce717 100644 --- a/gz_ros2_control_demos/package.xml +++ b/gz_ros2_control_demos/package.xml @@ -23,28 +23,30 @@ std_msgs ament_index_python - control_msgs - effort_controllers geometry_msgs - hardware_interface - gz_ros2_control - ros_gz_bridge - imu_sensor_broadcaster - joint_state_broadcaster - joint_trajectory_controller - launch launch_ros - ros2launch + launch rclcpp robot_state_publisher + ros_gz_bridge ros_gz_sim - ros2controlcli + ros2launch std_msgs - velocity_controllers + xacro + + + ackermann_steering_controller + control_msgs diff_drive_controller + effort_controllers + gz_ros2_control + hardware_interface + imu_sensor_broadcaster + joint_state_broadcaster + joint_trajectory_controller + ros2controlcli tricycle_controller - ackermann_steering_controller - xacro + velocity_controllers ament_cmake_gtest ament_lint_auto From efabf77b079c5286bf96d24da7cfb2db1fe910cb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 09:58:04 +0000 Subject: [PATCH 5/9] Update time of diff_drive_publisher --- gz_ros2_control_demos/examples/example_diff_drive.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gz_ros2_control_demos/examples/example_diff_drive.cpp b/gz_ros2_control_demos/examples/example_diff_drive.cpp index 365ecb59..6858564c 100644 --- a/gz_ros2_control_demos/examples/example_diff_drive.cpp +++ b/gz_ros2_control_demos/examples/example_diff_drive.cpp @@ -34,8 +34,6 @@ int main(int argc, char * argv[]) geometry_msgs::msg::TwistStamped command; - command.header.stamp = node->now(); - command.twist.linear.x = 0.1; command.twist.linear.y = 0.0; command.twist.linear.z = 0.0; @@ -45,6 +43,7 @@ int main(int argc, char * argv[]) command.twist.angular.z = 0.1; while (1) { + command.header.stamp = node->now(); publisher->publish(command); std::this_thread::sleep_for(50ms); rclcpp::spin_some(node); From 0beefbfe1b6633bd02245f98672c61d5600024d9 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 09:59:31 +0000 Subject: [PATCH 6/9] Update time of tricycle publisher --- gz_ros2_control_demos/examples/example_tricycle_drive.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp index 1783ce1e..ff49808b 100644 --- a/gz_ros2_control_demos/examples/example_tricycle_drive.cpp +++ b/gz_ros2_control_demos/examples/example_tricycle_drive.cpp @@ -34,8 +34,6 @@ int main(int argc, char * argv[]) geometry_msgs::msg::TwistStamped command; - command.header.stamp = node->now(); - command.twist.linear.x = 0.2; command.twist.linear.y = 0.0; command.twist.linear.z = 0.0; @@ -45,6 +43,7 @@ int main(int argc, char * argv[]) command.twist.angular.z = 0.1; while (1) { + command.header.stamp = node->now(); publisher->publish(command); std::this_thread::sleep_for(50ms); rclcpp::spin_some(node); From 07cba099d85d742511167c72cbec76927f64463e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 10:06:38 +0000 Subject: [PATCH 7/9] ERROR->INFO --- gz_ros2_control_demos/examples/example_position.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gz_ros2_control_demos/examples/example_position.cpp b/gz_ros2_control_demos/examples/example_position.cpp index 1b294db9..b0a6820f 100644 --- a/gz_ros2_control_demos/examples/example_position.cpp +++ b/gz_ros2_control_demos/examples/example_position.cpp @@ -154,7 +154,7 @@ int main(int argc, char * argv[]) node.reset(); return 1; } - RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + RCLCPP_INFO(node->get_logger(), "send goal call ok :)"); rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); @@ -164,7 +164,7 @@ int main(int argc, char * argv[]) node.reset(); return 1; } - RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + RCLCPP_INFO(node->get_logger(), "Goal was accepted by server"); // Wait for the server to be done with the goal auto result_future = action_client->async_get_result(goal_handle); From eafa3e76df1cfd70d7bd7027113306c07de99a95 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 10:11:39 +0000 Subject: [PATCH 8/9] Fix segfault of example_gripper --- gz_ros2_control_demos/examples/example_gripper.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gz_ros2_control_demos/examples/example_gripper.cpp b/gz_ros2_control_demos/examples/example_gripper.cpp index 449cf6ab..c534fcb5 100644 --- a/gz_ros2_control_demos/examples/example_gripper.cpp +++ b/gz_ros2_control_demos/examples/example_gripper.cpp @@ -23,13 +23,11 @@ #include "std_msgs/msg/float64_multi_array.hpp" -std::shared_ptr node; - int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - node = std::make_shared("gripper_test_node"); + std::shared_ptr node = std::make_shared("gripper_test_node"); auto publisher = node->create_publisher( "/gripper_controller/commands", 10); From 52a047378ca61f9656236bd29f4dd0c16c9b6384 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sat, 24 Aug 2024 10:17:33 +0000 Subject: [PATCH 9/9] Update xml launch files --- .../launch/gripper_mimic_joint_example_effort.launch.xml | 5 +++-- .../launch/gripper_mimic_joint_example_position.launch.xml | 5 +++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml index a1127a48..a83c4ac2 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_effort.launch.xml @@ -26,6 +26,7 @@ Author: Dr. Denis + @@ -50,8 +51,8 @@ Author: Dr. Denis - - + + diff --git a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml index 76dd181d..4cdc2be5 100644 --- a/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml +++ b/gz_ros2_control_demos/launch/gripper_mimic_joint_example_position.launch.xml @@ -26,6 +26,7 @@ Author: Dr. Denis + @@ -50,8 +51,8 @@ Author: Dr. Denis - - + +