Fix namespacing for multiple instances of gazebo_ros2_control plugin (#…

…181)

Co-authored-by: ben.holden <[email protected]>
Co-authored-by: Christoph Froehlich <[email protected]>
(cherry picked from commit 828b5f2)

doc/index.rst

@@ -313,6 +313,54 @@ The following is a basic configuration of the controllers:
 .. literalinclude:: ../gazebo_ros2_control_demos/config/cart_controller.yaml
    :language: yaml
 
+
+Multiple Namespaces
+-----------------------------------------------------------
+The gazebo_ros2_control plugin can be launched in multiple namespaces. This is useful if it is desired to have multiple robots of the same description running at the same time.
+It is possible to directly define the namespace in the URDF file, as shown below:
+
+.. code-block:: xml
+
+  <gazebo>
+    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+      <parameters> $(arg control_yaml) </parameters>
+      <ros>
+        <namespace>r1</namespace>
+        <remapping>/tf:=tf</remapping>
+        <remapping>/tf_static:=tf_static</remapping>
+        <remapping>diffdrive_controller/odom:=odom</remapping>
+      </ros>
+    </plugin>
+  </gazebo>
+
+Where ``r1`` is the namespace associated to the particular instance of the plugin. Please note that the remapping of ``/tf`` and ``/tf_static`` is recommended only if you want to have a separate TF buffer for each namespace, e.g ``/r1/tf``
+
+As an example, run the following command to launch the diff_drive example within the namespace ``r1``
+
+.. code-block:: bash
+
+  ros2 launch gazebo_ros2_control_demos diff_drive_namespaced.launch.py
+
+A second method to launch an instance with a particular namespace is by using the Gazebo ``spawn_entity.py`` tool and setting the ``-robot_namespace`` parameter to the script:
+
+.. code-block:: bash
+
+  ros2 run gazebo_ros spawn_entity.py -entity robot_1 -robot_namespace r1 -topic robot_description
+
+The script calls the ``spawn_entity`` service to spawn an entity named ``robot_1`` in the namespace ``r1``.  It is assumed that a robot description is being published by a ``robot_state_publisher`` node on the topic ``robot_description``, this will also be used for the ``gazebo_ros2_control`` plugin.
+
+The ``spawn_entity.py`` script will rewrite the robot description to include a namespace for every plugin tag. Please note that this method will apply a namespace to every plugin in the robot description. It will also overwrite any existing namespaces set in the description file.
+
+As an example, run the following command to spawn two diff_drive robots in the namespaces ``r1`` and ``r2``
+
+.. note::
+
+  The ros2_control settings for the controller_manager and the controller defined in ``diff_drive_controller_namespaced.yaml`` use wildcards to match both namespaces.
+
+.. code-block:: bash
+
+  ros2 launch gazebo_ros2_control_demos diff_drive_pair_namespaced.launch.py
+
 gazebo_ros2_control_demos
 ==========================================
 

gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp

@@ -226,24 +226,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
 
   if (sdf->HasElement("ros")) {
     sdf = sdf->GetElement("ros");
-
-    // Set namespace if tag is present
-    if (sdf->HasElement("namespace")) {
-      std::string ns = sdf->GetElement("namespace")->Get<std::string>();
-      // prevent exception: namespace must be absolute, it must lead with a '/'
-      if (ns.empty() || ns[0] != '/') {
-        ns = '/' + ns;
-      }
-      std::string ns_arg = std::string("__ns:=") + ns;
-      arguments.push_back(RCL_REMAP_FLAG);
-      arguments.push_back(ns_arg);
-    }
-
     // Get list of remapping rules from SDF
     if (sdf->HasElement("remapping")) {
       sdf::ElementPtr argument_sdf = sdf->GetElement("remapping");
 
-      arguments.push_back(RCL_ROS_ARGS_FLAG);
       while (argument_sdf) {
         std::string argument = argument_sdf->Get<std::string>();
         arguments.push_back(RCL_REMAP_FLAG);

gazebo_ros2_control_demos/config/diff_drive_controller_namespaced.yaml

@@ -0,0 +1,50 @@
+/**/controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+    use_sim_time: true
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    diff_drive_base_controller:
+      type: diff_drive_controller/DiffDriveController
+
+/**/diff_drive_base_controller:
+  ros__parameters:
+    left_wheel_names: ["left_wheel_joint"]
+    right_wheel_names: ["right_wheel_joint"]
+
+    wheel_separation: 1.25
+    wheel_radius: 0.3
+
+    wheel_separation_multiplier: 1.0
+    left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
+
+    publish_rate: 50.0
+    odom_frame_id: odom
+    base_frame_id: chassis
+    pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01]
+    twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01]
+
+    open_loop: true
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    #publish_limited_velocity: true
+    #velocity_rolling_window_size: 10
+
+    # Velocity and acceleration limits
+    # Whenever a min_* is unspecified, default to -max_*
+    linear.x.max_velocity: 1.0
+    linear.x.min_velocity: -1.0
+    linear.x.max_acceleration: 1.0
+    linear.x.max_jerk: 0.0
+    linear.x.min_jerk: 0.0
+
+    angular.z.max_velocity: 1.0
+    angular.z.min_velocity: -1.0
+    angular.z.max_acceleration: 1.0
+    angular.z.min_acceleration: -1.0
+    angular.z.max_jerk: 0.0
+    angular.z.min_jerk: 0.0

gazebo_ros2_control_demos/launch/diff_drive_namespaced.launch.py

@@ -0,0 +1,91 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_diff_drive_namespaced.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        namespace='r1',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
+                        namespace='r1',
+                        arguments=['-topic', 'robot_description',
+                                   '-entity', 'diffbot'],
+                        output='screen')
+
+    load_joint_state_broadcaster = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster',
+             '-c', '/r1/controller_manager'],
+        output='screen'
+    )
+
+    load_diff_drive_base_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'diff_drive_base_controller',
+             '-c', '/r1/controller_manager'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=spawn_entity,
+                on_exit=[load_joint_state_broadcaster],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_joint_state_broadcaster,
+                on_exit=[load_diff_drive_base_controller],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher,
+        spawn_entity,
+    ])

gazebo_ros2_control_demos/launch/diff_drive_pair_namespaced.launch.py

@@ -0,0 +1,135 @@
+# Copyright 2024 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+from launch_ros.actions import Node
+
+import xacro
+
+
+def generate_launch_description():
+    gazebo = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource([os.path.join(
+                    get_package_share_directory('gazebo_ros'), 'launch'), '/gazebo.launch.py']),
+             )
+
+    gazebo_ros2_control_demos_path = os.path.join(
+        get_package_share_directory('gazebo_ros2_control_demos'))
+
+    xacro_file = os.path.join(gazebo_ros2_control_demos_path,
+                              'urdf',
+                              'test_diff_drive_namespaced.xacro.urdf')
+
+    doc = xacro.parse(open(xacro_file))
+    xacro.process_doc(doc)
+    params = {'robot_description': doc.toxml()}
+
+    node_robot_state_publisher_r1 = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        namespace='r1',
+        output='screen',
+        parameters=[params]
+    )
+
+    node_robot_state_publisher_r2 = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        namespace='r2',
+        output='screen',
+        parameters=[params]
+    )
+
+    spawn_entity_r1 = Node(
+      package='gazebo_ros', executable='spawn_entity.py',
+      arguments=[
+                '-topic', '/r1/robot_description',
+                '-robot_namespace', 'r1',
+                '-entity', 'diffbot1'
+                ],
+      output='screen'
+    )
+
+    spawn_entity_r2 = Node(
+      package='gazebo_ros', executable='spawn_entity.py',
+      arguments=[
+                '-topic', '/r2/robot_description',
+                '-robot_namespace', 'r2',
+                '-entity', 'diffbot1'
+                ],
+      output='screen'
+    )
+
+    load_joint_state_broadcaster_r1 = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster',
+             '-c', '/r1/controller_manager'],
+        output='screen'
+    )
+
+    load_diff_drive_base_controller_r1 = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'diff_drive_base_controller',
+             '-c', '/r1/controller_manager'],
+        output='screen'
+    )
+
+    load_joint_state_broadcaster_r2 = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'joint_state_broadcaster',
+             '-c', '/r2/controller_manager'],
+        output='screen'
+    )
+
+    load_diff_drive_base_controller_r2 = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
+             'diff_drive_base_controller',
+             '-c', '/r2/controller_manager'],
+        output='screen'
+    )
+
+    return LaunchDescription([
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                  target_action=spawn_entity_r1,
+                  on_exit=[
+                            load_joint_state_broadcaster_r1,
+                            load_diff_drive_base_controller_r1
+                          ],
+            )
+        ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                  target_action=spawn_entity_r2,
+                  on_exit=[
+                            load_joint_state_broadcaster_r2,
+                            load_diff_drive_base_controller_r2
+                          ],
+            )
+        ),
+        gazebo,
+        node_robot_state_publisher_r1,
+        node_robot_state_publisher_r2,
+        spawn_entity_r1,
+        spawn_entity_r2,
+    ])