Add Demos for SDF (#427) (#465)

Signed-off-by: Aarav Gupta <[email protected]> Signed-off-by: Aarav Gupta <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> (cherry picked from commit d2b2f0d) Co-authored-by: Aarav Gupta <[email protected]>
ros-controls · Dec 23, 2024 · 5d116f8 · 5d116f8
1 parent 071d61d
commit 5d116f8
Show file tree

Hide file tree

Showing 6 changed files with 709 additions and 51 deletions.
diff --git 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 ``<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
 -----------------------------------------------------------

diff --git 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
@@ -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
@@ -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
-Original file line number
+Diff line change
@@ Expand Up / @@ -25,6 +25,7 @@ find_package(geometry_msgs REQUIRED) @@
     install(DIRECTORY
       launch
       config
+      sdf
       urdf
       DESTINATION share/${PROJECT_NAME}/
     )
@@ Expand Down @@