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..f215830f 100644 --- a/gz_ros2_control_demos/config/cart_controller_velocity.yaml +++ b/gz_ros2_control_demos/config/cart_controller_velocity.yaml @@ -2,21 +2,17 @@ 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: + type: velocity_controllers/JointGroupVelocityController joints: - slider_to_cart 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/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); 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); 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); 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); 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); 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_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.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/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 - - + + 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_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 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()