diff --git a/doc/index.rst b/doc/index.rst index 13db3e1..1f53c11 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -61,13 +61,13 @@ To run the demo ros2 run gz_ros2_control_demos example_position -Add ros2_control tag to a URDF +Add ros2_control tag to a URDF or SDF ========================================== Simple setup ----------------------------------------------------------- -To use *ros2_control* with your robot, you need to add some additional elements to your URDF. +To use *ros2_control* with your robot, you need to add some additional elements to your URDF or SDF. You should include the tag ```` to access and control the robot interfaces. We should include: @@ -96,7 +96,7 @@ include: Using mimic joints in simulation ----------------------------------------------------------- -To use ``mimic`` joints in *gz_ros2_control* you should define its parameters in your URDF, i.e, set the ```` tag to the mimicked joint (see the `URDF specification `__) +To use ``mimic`` joints in *gz_ros2_control* you should define its parameters in your URDF or SDF, i.e, set the ```` tag to the mimicked joint (see the `URDF specification `__ or the `SDF specification `__) .. code-block:: xml @@ -122,12 +122,14 @@ The mimic joint must not have command interfaces configured in the `` @@ -136,12 +138,20 @@ robot hardware interfaces between *ros2_control* and Gazebo. +SDF: + +.. code-block:: xml + + + $(find gz_ros2_control_demos)/config/cart_controller.yaml + + The *gz_ros2_control* ```` tag also has the following optional child elements: * ````: A YAML file with the configuration of the controllers. This element can be given multiple times to load multiple files. * ````: Set controller manager name (default: ``controller_manager``) -The following additional parameters can be set via child elements in the URDF or via ROS parameters in the YAML file above: +The following additional parameters can be set via child elements in the URDF/SDF or via ROS parameters in the YAML file above: * ````: if set to true (default), it will hold the joints' position if their interface was not claimed, e.g., the controller hasn't been activated yet. * ````: Set the proportional gain. (default: 0.1) This determines the setpoint for a position-controlled joint ``joint_velocity = joint_position_error * position_proportional_gain``. @@ -157,6 +167,8 @@ or via ROS parameters: Additionally, one can specify a namespace and remapping rules, which will be forwarded to the controller_manager and loaded controllers. Add the following ```` section: +URDF: + .. code-block:: xml @@ -169,10 +181,23 @@ Additionally, one can specify a namespace and remapping rules, which will be for +SDF: + +.. code-block:: xml + + + + ... + + my_namespace + /robot_description:=/robot_description_full + + + Default gz_ros2_control Behavior ----------------------------------------------------------- -By default, without a ```` tag, *gz_ros2_control* will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. +By default, without a ```` tag, *gz_ros2_control* will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF or SDF. This is sufficient for most cases, and good for at least getting started. The default behavior provides the following ros2_control interfaces: @@ -188,9 +213,11 @@ The *gz_ros2_control* Gazebo plugin also provides a pluginlib-based interface to These plugins must inherit the ``gz_ros2_control::GazeboSimSystemInterface``, which implements a simulated *ros2_control* ``hardware_interface::SystemInterface``. SystemInterface provides API-level access to read and command joint properties. -The respective GazeboSimSystemInterface sub-class is specified in a URDF model and is loaded when the +The respective GazeboSimSystemInterface sub-class is specified in a URDF or SDF model and is loaded when the robot model is loaded. For example, the following XML will load the default plugin: +URDF: + .. code-block:: xml @@ -205,12 +232,28 @@ robot model is loaded. For example, the following XML will load the default plug +SDF: + +.. code-block:: xml + + + + gz_ros2_control/GazeboSimSystem + + ... + + + ... + + Set up controllers ----------------------------------------------------------- Use the tag ```` inside ```` to set the YAML file with the controller configuration and use the tag ```` to set the controller manager node name. +URDF: + .. code-block:: xml @@ -220,6 +263,15 @@ and use the tag ```` to set the controller +SDF: + +.. code-block:: xml + + + $(find gz_ros2_control_demos)/config/cart_controller.yaml + controller_manager + + The following is a basic configuration of the controllers: - ``joint_state_broadcaster``: This controller publishes the state of all resources registered to a ``hardware_interface::StateInterface`` to a topic of type ``sensor_msgs/msg/JointState``. @@ -233,6 +285,10 @@ gz_ros2_control_demos ========================================== There are some examples in the *gz_ros2_control_demos* package. +To specify whether to use URDF or SDF, you can launch the demo in the following way (the default is URDF): +.. code-block:: shell + + ros2 launch gz_ros2_control_demos description_format:=sdf Cart on rail ----------------------------------------------------------- diff --git a/gz_ros2_control_demos/CMakeLists.txt b/gz_ros2_control_demos/CMakeLists.txt index b991ac2..f962231 100644 --- a/gz_ros2_control_demos/CMakeLists.txt +++ b/gz_ros2_control_demos/CMakeLists.txt @@ -25,6 +25,7 @@ find_package(geometry_msgs REQUIRED) install(DIRECTORY launch config + sdf urdf DESTINATION share/${PROJECT_NAME}/ ) 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 fb6f065..ad8d596 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, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -27,18 +27,29 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - PathJoinSubstitution( - [FindPackageShare('gz_ros2_control_demos'), - 'urdf', 'test_ackermann_drive.xacro.urdf'] - ), - ] - ) - robot_description = {'robot_description': robot_description_content} + def robot_state_publisher(context): + performed_description_format = LaunchConfiguration('description_format').perform(context) + # Get URDF or SDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + FindPackageShare('gz_ros2_control_demos'), + performed_description_format, + f'test_ackermann_drive.xacro.{performed_description_format}' + ]), + ] + ) + robot_description = {'robot_description': robot_description_content} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + return [node_robot_state_publisher] + robot_controllers = PathJoinSubstitution( [ FindPackageShare('gz_ros2_control_demos'), @@ -47,13 +58,6 @@ def generate_launch_description(): ] ) - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[robot_description] - ) - gz_spawn_entity = Node( package='ros_gz_sim', executable='create', @@ -84,7 +88,7 @@ def generate_launch_description(): output='screen' ) - return LaunchDescription([ + ld = LaunchDescription([ bridge, # Launch gazebo environment IncludeLaunchDescription( @@ -105,11 +109,16 @@ def generate_launch_description(): on_exit=[ackermann_steering_controller_spawner], ) ), - node_robot_state_publisher, gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', default_value=use_sim_time, description='If true, use simulated clock'), + DeclareLaunchArgument( + 'description_format', + default_value='urdf', + description='Robot description format to use, urdf or sdf'), ]) + ld.add_action(OpaqueFunction(function=robot_state_publisher)) + return ld 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 21c5afb..fb57396 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, IncludeLaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.actions import RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource @@ -27,18 +27,29 @@ def generate_launch_description(): # Launch Arguments use_sim_time = LaunchConfiguration('use_sim_time', default=True) - # Get URDF via xacro - robot_description_content = Command( - [ - PathJoinSubstitution([FindExecutable(name='xacro')]), - ' ', - PathJoinSubstitution( - [FindPackageShare('gz_ros2_control_demos'), - 'urdf', 'test_diff_drive.xacro.urdf'] - ), - ] - ) - robot_description = {'robot_description': robot_description_content} + def robot_state_publisher(context): + performed_description_format = LaunchConfiguration('description_format').perform(context) + # Get URDF or SDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name='xacro')]), + ' ', + PathJoinSubstitution([ + FindPackageShare('gz_ros2_control_demos'), + performed_description_format, + f'test_diff_drive.xacro.{performed_description_format}' + ]), + ] + ) + robot_description = {'robot_description': robot_description_content} + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description] + ) + return [node_robot_state_publisher] + robot_controllers = PathJoinSubstitution( [ FindPackageShare('gz_ros2_control_demos'), @@ -47,13 +58,6 @@ def generate_launch_description(): ] ) - node_robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[robot_description] - ) - gz_spawn_entity = Node( package='ros_gz_sim', executable='create', @@ -85,7 +89,7 @@ def generate_launch_description(): output='screen' ) - return LaunchDescription([ + ld = LaunchDescription([ # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -106,11 +110,16 @@ def generate_launch_description(): ) ), bridge, - node_robot_state_publisher, gz_spawn_entity, # Launch Arguments DeclareLaunchArgument( 'use_sim_time', default_value=use_sim_time, description='If true, use simulated clock'), + DeclareLaunchArgument( + 'description_format', + default_value='urdf', + description='Robot description format to use, urdf or sdf'), ]) + ld.add_action(OpaqueFunction(function=robot_state_publisher)) + return ld diff --git a/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf new file mode 100644 index 0000000..bca986d --- /dev/null +++ b/gz_ros2_control_demos/sdf/test_ackermann_drive.xacro.sdf @@ -0,0 +1,364 @@ + + + + + true + + + + + + + + + + 2 1 0.5 + + + + + + + + 2 1 0.5 + + + + 1 0.5088 0.0468 1 + 1 0.5088 0.0468 1 + + + + + 1 + + 0.126164 + 0.0 + 0.0 + 0.416519 + 0.0 + 0.481014 + + + + + + base_link + chassis + 0.8 0 0.5 0 0 0 + + + + + + + + + + 0.3 + 0.08 + + + + + + + + 0.3 + 0.08 + + + + 0 0 0 1 + 0 0 0 1 + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + chassis + rear_left_wheel + -0.8 0.5 -0.2 -1.57 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + + + + + + 0.3 + 0.08 + + + + + + + + 0.3 + 0.08 + + + + 0 0 0 1 + 0 0 0 1 + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + chassis + rear_right_wheel + -0.8 -0.5 -0.2 -1.57 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + + + 0.1 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + chassis + left_wheel_steering + 0.9 0.5 -0.2 -1.57 0 0 + + 0 1 0 + + -inf + inf + + + 0.2 + + + + + + + + + 0.1 + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + chassis + right_wheel_steering + 0.9 -0.5 -0.2 -1.57 0 0 + + 0 1 0 + + -inf + inf + + + 0.2 + + + + + + + + + + + + 0.3 + 0.08 + + + + + + + + 0.3 + 0.08 + + + + 0 0 0 1 + 0 0 0 1 + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + left_wheel_steering + front_left_wheel + 0 0 0 0 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + + + + + + 0.3 + 0.08 + + + + + + + + 0.3 + 0.08 + + + + 0 0 0 1 + 0 0 0 1 + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + right_wheel_steering + front_right_wheel + 0 0 0 0 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + + + + + + $(find gz_ros2_control_demos)/config/ackermann_drive_controller.yaml + + + + diff --git a/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf new file mode 100644 index 0000000..0e7f2ba --- /dev/null +++ b/gz_ros2_control_demos/sdf/test_diff_drive.xacro.sdf @@ -0,0 +1,219 @@ + + + + + true + + + + base_link + chassis + -0.151427 -0 0.5 0 0 0 + + + + + + + + 2.01142 1 0.568726 + + + + 1 0.5088 0.0468 1 + 1 0.5088 0.0468 1 + + + + + + + 2.01142 1 0.568726 + + + + + + 1.14395 + + 0.126164 + 0.0 + 0.0 + 0.416519 + 0.0 + 0.481014 + + + + + + chassis + left_wheel + 0.554283 0.625029 -0.3 -1.5707 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + + + + 0.3 + + + + 0 0 0 1 + 0 0 0 1 + + + + + + + 0.3 + + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + chassis + right_wheel + 0.554283 -0.625029 -0.3 -1.5707 0 0 + + 0 0 1 + + -inf + inf + + + 0.2 + + + + + + + + + + 0.3 + + + + 0 0 0 1 + 0 0 0 1 + + + + + + + 0.3 + + + + + + 2 + + 0.145833 + 0.0 + 0.0 + 0.145833 + 0.0 + 0.125 + + + + + + chassis + caster + -0.80571 0 -0.2 0 0 0 + + + + + + + + 0.2 + + + + 1 1 1 1 + 1 1 1 1 + + + + + + + 0.2 + + + + + + 0.005 + + 0.1 + 0.0 + 0.0 + 0.1 + 0.0 + 0.1 + + + + + + + gz_ros2_control/GazeboSimSystem + + + + -1 + 1 + + + + + + + -1 + 1 + + + + + + + + $(find gz_ros2_control_demos)/config/diff_drive_controller_velocity.yaml + + + +