diff --git a/doc/index.rst b/doc/index.rst
index 81db92d..9b0d39a 100644
--- a/doc/index.rst
+++ b/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
+
+
+
+ $(arg control_yaml)
+
+ r1
+ /tf:=tf
+ /tf_static:=tf_static
+ diffdrive_controller/odom:=odom
+
+
+
+
+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
==========================================
diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
index f9b9990..5ab3643 100644
--- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
+++ b/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();
- // 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();
arguments.push_back(RCL_REMAP_FLAG);
diff --git a/gazebo_ros2_control_demos/config/diff_drive_controller_namespaced.yaml b/gazebo_ros2_control_demos/config/diff_drive_controller_namespaced.yaml
new file mode 100644
index 0000000..424a4a6
--- /dev/null
+++ b/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
diff --git a/gazebo_ros2_control_demos/launch/diff_drive_namespaced.launch.py b/gazebo_ros2_control_demos/launch/diff_drive_namespaced.launch.py
new file mode 100644
index 0000000..ade6623
--- /dev/null
+++ b/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,
+ ])
diff --git a/gazebo_ros2_control_demos/launch/diff_drive_pair_namespaced.launch.py b/gazebo_ros2_control_demos/launch/diff_drive_pair_namespaced.launch.py
new file mode 100644
index 0000000..722b083
--- /dev/null
+++ b/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,
+ ])
diff --git a/gazebo_ros2_control_demos/urdf/test_diff_drive_namespaced.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_diff_drive_namespaced.xacro.urdf
new file mode 100644
index 0000000..3ea0027
--- /dev/null
+++ b/gazebo_ros2_control_demos/urdf/test_diff_drive_namespaced.xacro.urdf
@@ -0,0 +1,167 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ gazebo_ros2_control/GazeboSystem
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+ -1
+ 1
+
+
+
+
+
+
+
+
+
+
+ r1
+
+ $(find gazebo_ros2_control_demos)/config/diff_drive_controller_namespaced.yaml
+
+
+
+