Merge branch 'rolling' into rm/iron

.pre-commit-config.yaml

@@ -136,7 +136,7 @@ repos:
         exclude: CHANGELOG\.rst|\.(svg|pyc)$
 
   - repo: https://github.com/python-jsonschema/check-jsonschema
-    rev: 0.29.4
+    rev: 0.30.0
     hooks:
       - id: check-github-workflows
         args: ["--verbose"]

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 ``<ros2_control>`` 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 ``<mimic>`` tag to the mimicked joint (see the `URDF specification <https://wiki.ros.org/urdf/XML/joint>`__)
+To use ``mimic`` joints in *gz_ros2_control* you should define its parameters in your URDF or SDF, i.e, set the ``<mimic>`` tag to the mimicked joint (see the `URDF specification <https://wiki.ros.org/urdf/XML/joint>`__ or the `SDF specification <http://sdformat.org/spec?ver=1.11&elem=joint#axis_mimic>`__)
 
 .. code-block:: xml
 
@@ -122,12 +122,14 @@ The mimic joint must not have command interfaces configured in the ``<ros2_contr
 Add the gz_ros2_control plugin
 ==========================================
 
-In addition to the *ros2_control* tags, a Gazebo plugin needs to be added to your URDF that
+In addition to the *ros2_control* tags, a Gazebo plugin needs to be added to your URDF or SDF that
 actually parses the *ros2_control* tags and loads the appropriate hardware interfaces and
 controller manager. By default the *gz_ros2_control* plugin is very simple, though it is also
 extensible via an additional plugin architecture to allow power users to create their own custom
 robot hardware interfaces between *ros2_control* and Gazebo.
 
+URDF:
+
 .. code-block:: xml
 
   <gazebo>
@@ -136,12 +138,20 @@ robot hardware interfaces between *ros2_control* and Gazebo.
     </plugin>
   </gazebo>
 
+SDF:
+
+.. code-block:: xml
+
+  <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
+  </plugin>
+
 The *gz_ros2_control* ``<plugin>`` tag also has the following optional child elements:
 
 * ``<parameters>``: A YAML file with the configuration of the controllers. This element can be given multiple times to load multiple files.
 * ``<controller_manager_name>``: 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:
 
 * ``<hold_joints>``: 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.
 * ``<position_proportional_gain>``: 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 ``<ros>`` section:
 
+URDF:
+
 .. code-block:: xml
 
   <gazebo>
@@ -169,10 +181,23 @@ Additionally, one can specify a namespace and remapping rules, which will be for
     </plugin>
   </gazebo>
 
+SDF:
+
+.. code-block:: xml
+
+
+  <plugin filename="libgz_ros2_control-system.so" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+    ...
+    <ros>
+      <namespace>my_namespace</namespace>
+      <remapping>/robot_description:=/robot_description_full</remapping>
+    </ros>
+  </plugin>
+
 Default gz_ros2_control Behavior
 -----------------------------------------------------------
 
-By default, without a ``<plugin>`` 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 ``<plugin>`` 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
 
   <ros2_control name="GazeboSimSystem" type="system">
@@ -205,12 +232,28 @@ robot model is loaded. For example, the following XML will load the default plug
     </plugin>
   </gazebo>
 
+SDF:
+
+.. code-block:: xml
+
+  <ros2_control name="GazeboSimSystem" type="system">
+    <hardware>
+      <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+    </hardware>
+    ...
+  <ros2_control>
+  <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+    ...
+  </plugin>
+
 Set up controllers
 -----------------------------------------------------------
 
 Use the tag ``<parameters>`` inside ``<plugin>`` to set the YAML file with the controller configuration
 and use the tag ``<controller_manager_prefix_node_name>`` to set the controller manager node name.
 
+URDF:
+
 .. code-block:: xml
 
   <gazebo>
@@ -220,6 +263,15 @@ and use the tag ``<controller_manager_prefix_node_name>`` to set the controller
     </plugin>
   <gazebo>
 
+SDF:
+
+.. code-block:: xml
+
+  <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+    <parameters>$(find gz_ros2_control_demos)/config/cart_controller.yaml</parameters>
+    <controller_manager_prefix_node_name>controller_manager</controller_manager_prefix_node_name>
+  </plugin>
+
 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 <launch file> description_format:=sdf
 
 Cart on rail
 -----------------------------------------------------------

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}/
 )

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

gz_ros2_control_demos/launch/cart_example_effort.launch.py

@@ -77,6 +77,14 @@ def generate_launch_description():
             ],
     )
 
+    # Bridge
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
     return LaunchDescription([
         # Launch gazebo environment
         IncludeLaunchDescription(
@@ -97,6 +105,7 @@ def generate_launch_description():
                 on_exit=[effort_controller_spawner],
             )
         ),
+        bridge,
         node_robot_state_publisher,
         gz_spawn_entity,
         # Launch Arguments

gz_ros2_control_demos/launch/cart_example_position.launch.py

@@ -65,7 +65,8 @@ def generate_launch_description():
     joint_state_broadcaster_spawner = Node(
         package='controller_manager',
         executable='spawner',
-        arguments=['joint_state_broadcaster'],
+        arguments=['joint_state_broadcaster',
+                   ],
     )
     joint_trajectory_controller_spawner = Node(
         package='controller_manager',
@@ -77,6 +78,14 @@ def generate_launch_description():
             ],
     )
 
+    # Bridge
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
     return LaunchDescription([
         # Launch gazebo environment
         IncludeLaunchDescription(
@@ -97,6 +106,7 @@ def generate_launch_description():
                 on_exit=[joint_trajectory_controller_spawner],
             )
         ),
+        bridge,
         node_robot_state_publisher,
         gz_spawn_entity,
         # Launch Arguments

gz_ros2_control_demos/launch/cart_example_velocity.launch.py

@@ -86,6 +86,14 @@ def generate_launch_description():
             ],
     )
 
+    # Bridge
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
     return LaunchDescription([
         # Launch gazebo environment
         IncludeLaunchDescription(
@@ -107,6 +115,7 @@ def generate_launch_description():
                          imu_sensor_broadcaster_spawner],
             )
         ),
+        bridge,
         node_robot_state_publisher,
         gz_spawn_entity,
         # Launch Arguments