Add entity_namespace support for spawned entities

ros_gz_sim/doc/executables.rst

@@ -33,8 +33,14 @@ for argument parsing (``--helpshort`` for the full list).
        --world default \
        --file https://fuel.gazebosim.org/1.0/openrobotics/models/Gazebo \
        --name my_gazebo \
+       --ns my_gazebo \
        --x 0 --y 0 --z 0.5
 
+Use ``--ns`` to set the namespace for the spawned entity. If ``--ns`` is not
+set or is set to an empty string, the namespace behavior follows the source SDF
+file. The ``{name}`` placeholder is also supported and will be replaced by the
+final spawned entity name.
+
 ``create`` is preferred over ``spawn_entity`` when you already have a running
 Gazebo transport channel and do not want the ROS service layer.
 

ros_gz_sim/doc/launch_actions.rst

@@ -92,6 +92,7 @@ XML form
          world="default"
          file="$(find-pkg-share my_pkg)/models/robot.sdf"
          entity_name="robot"
+         entity_namespace="robot"
          x="0.0" y="0.0" z="0.5"
          yaw="1.5707"/>
    </launch>
@@ -107,6 +108,7 @@ Python form
        world='default',
        file='/path/to/robot.sdf',
        entity_name='robot',
+       entity_namespace='robot',
        x='0.0', y='0.0', z='0.5', yaw='1.5707',
    )
 
@@ -129,6 +131,11 @@ Arguments
 ``entity_name``
     Desired entity name in the simulation.
 
+``entity_namespace``
+    Namespace for the spawned entity. If empty or not set, the namespace
+    behavior follows the source SDF file. The ``{name}`` placeholder is also
+    supported and will be replaced by the final spawned entity name.
+
 ``allow_renaming``
     If ``true``, Gazebo appends a suffix when ``entity_name`` is already
     taken.

ros_gz_sim/launch/gz_spawn_model.launch

@@ -4,6 +4,7 @@
   <arg name="model_string" default="" />
   <arg name="topic" default="" />
   <arg name="entity_name" default="" />
+  <arg name="entity_namespace" default="" />
   <arg name="allow_renaming" default="False" />
   <arg name="x" default="" />
   <arg name="y" default="" />
@@ -17,6 +18,7 @@
     model_string="$(var model_string)"
     topic="$(var topic)"
     entity_name="$(var entity_name)"
+    entity_namespace="$(var entity_namespace)"
     allow_renaming="$(var allow_renaming)"
     x="$(var x)"
     y="$(var y)"

ros_gz_sim/launch/gz_spawn_model.launch.py

@@ -27,6 +27,7 @@ def generate_launch_description():
     model_string = LaunchConfiguration('model_string')
     topic = LaunchConfiguration('topic')
     entity_name = LaunchConfiguration('entity_name')
+    entity_namespace = LaunchConfiguration('entity_namespace')
     allow_renaming = LaunchConfiguration('allow_renaming')
     x = LaunchConfiguration('x', default='0.0')
     y = LaunchConfiguration('y', default='0.0')
@@ -54,6 +55,10 @@ def generate_launch_description():
         'entity_name', default_value=TextSubstitution(text=''),
         description='Name of the entity'
     )
+    declare_entity_namespace_cmd = DeclareLaunchArgument(
+        'entity_namespace', default_value=TextSubstitution(text=''),
+        description='Namespace for the spawned entity.'
+    )
     declare_allow_renaming_cmd = DeclareLaunchArgument(
         'allow_renaming', default_value='False',
         description='Whether the entity allows renaming or not'
@@ -68,6 +73,7 @@ def generate_launch_description():
                      'string': model_string,
                      'topic': topic,
                      'name': entity_name,
+                     'ns': entity_namespace,
                      'allow_renaming': allow_renaming,
                      'x': x,
                      'y': y,
@@ -87,6 +93,7 @@ def generate_launch_description():
     ld.add_action(declare_model_string_cmd)
     ld.add_action(declare_topic_cmd)
     ld.add_action(declare_entity_name_cmd)
+    ld.add_action(declare_entity_namespace_cmd)
     ld.add_action(declare_allow_renaming_cmd)
     # Add the actions to launch all of the create nodes
     ld.add_action(load_nodes)

ros_gz_sim/launch/ros_gz_spawn_model.launch

@@ -13,6 +13,7 @@
   <arg name="model_string" default="" />
   <arg name="topic" default="" />
   <arg name="entity_name" default="" />
+  <arg name="entity_namespace" default="" />
   <arg name="allow_renaming" default="False" />
   <arg name="x" default="0.0" />
   <arg name="y" default="0.0" />
@@ -39,6 +40,7 @@
     model_string="$(var model_string)"
     topic="$(var topic)"
     entity_name="$(var entity_name)"
+    entity_namespace="$(var entity_namespace)"
     allow_renaming="$(var allow_renaming)"
     x="$(var x)"
     y="$(var y)"

ros_gz_sim/launch/ros_gz_spawn_model.launch.py

@@ -39,6 +39,7 @@ def generate_launch_description():
     model_string = LaunchConfiguration('model_string')
     topic = LaunchConfiguration('topic')
     entity_name = LaunchConfiguration('entity_name')
+    entity_namespace = LaunchConfiguration('entity_namespace')
     allow_renaming = LaunchConfiguration('allow_renaming')
     x = LaunchConfiguration('x', default='0.0')
     y = LaunchConfiguration('y', default='0.0')
@@ -113,6 +114,11 @@ def generate_launch_description():
         description='Name of the entity'
     )
 
+    declare_entity_namespace_cmd = DeclareLaunchArgument(
+        'entity_namespace', default_value=TextSubstitution(text=''),
+        description='Namespace for the spawned entity.'
+    )
+
     declare_allow_renaming_cmd = DeclareLaunchArgument(
         'allow_renaming', default_value='False',
         description='Whether the entity allows renaming or not'
@@ -140,6 +146,7 @@ def generate_launch_description():
                           ('model_string', model_string),
                           ('topic', topic),
                           ('entity_name', entity_name),
+                          ('entity_namespace', entity_namespace),
                           ('allow_renaming', allow_renaming),
                           ('x', x),
                           ('y', y),
@@ -166,6 +173,7 @@ def generate_launch_description():
     ld.add_action(declare_model_string_cmd)
     ld.add_action(declare_topic_cmd)
     ld.add_action(declare_entity_name_cmd)
+    ld.add_action(declare_entity_namespace_cmd)
     ld.add_action(declare_allow_renaming_cmd)
     # Add the actions to launch all of the bridge + spawn_model nodes
     ld.add_action(ros_gz_bridge_action)

ros_gz_sim/ros_gz_sim/actions/gz_spawn_model.py

@@ -39,6 +39,7 @@ def __init__(
         model_string: Optional[SomeSubstitutionsType] = '',
         topic: Optional[SomeSubstitutionsType] = '',
         entity_name: Optional[SomeSubstitutionsType] = '',
+        entity_namespace: Optional[SomeSubstitutionsType] = '',
         allow_renaming: Optional[SomeSubstitutionsType] = 'False',
         x: Optional[SomeSubstitutionsType] = '0.0',
         y: Optional[SomeSubstitutionsType] = '0.0',
@@ -59,6 +60,7 @@ def __init__(
         :param: model_string XML(SDF) string.
         :param: topic Get XML from this topic.
         :param: entity_name Name of the entity.
+        :param: entity_namespace Namespace for the spawned entity.
         :param: allow_renaming Whether the entity allows renaming or not.
         :param: x X coordinate.
         :param: y Y coordinate.
@@ -73,6 +75,7 @@ def __init__(
         self.__model_string = model_string
         self.__topic = topic
         self.__entity_name = entity_name
+        self.__entity_namespace = entity_namespace
         self.__allow_renaming = allow_renaming
         self.__x = x
         self.__y = y
@@ -110,6 +113,10 @@ def parse(cls, entity: Entity, parser: Parser):
             'allow_renaming', data_type=str,
             optional=True)
 
+        entity_namespace = entity.get_attr(
+            'entity_namespace', data_type=str,
+            optional=True)
+
         x = entity.get_attr(
             'x', data_type=str,
             optional=True)
@@ -158,6 +165,10 @@ def parse(cls, entity: Entity, parser: Parser):
             allow_renaming = parser.parse_substitution(allow_renaming)
             kwargs['allow_renaming'] = allow_renaming
 
+        if isinstance(entity_namespace, str):
+            entity_namespace = parser.parse_substitution(entity_namespace)
+            kwargs['entity_namespace'] = entity_namespace
+
         if isinstance(x, str):
             x = parser.parse_substitution(x)
             kwargs['x'] = x
@@ -196,12 +207,13 @@ def execute(self, context: LaunchContext) -> Optional[List[Action]]:
                               ('model_string',   self.__model_string),
                               ('topic',  self.__topic),
                               ('entity_name', self.__entity_name),
+                              ('entity_namespace', self.__entity_namespace),
                               ('allow_renaming', self.__allow_renaming),
                               ('x',   self.__x),
                               ('y',  self.__y),
                               ('z', self.__z),
                               ('roll', self.__roll),
                               ('pitch',   self.__pitch),
-                              ('yaw',  self.__yaw), ])
+                              ('yaw',  self.__yaw)])
 
         return [gz_spawn_model_description]

ros_gz_sim/src/create.cpp

@@ -16,7 +16,7 @@
 
 #include <gz/msgs/boolean.pb.h>
 #include <gz/msgs/entity.pb.h>
-#include <gz/msgs/entity_factory.pb.h>
+#include <gz/msgs/entity_factory_with_ns.pb.h>
 #include <gz/msgs/stringmsg_v.pb.h>
 
 #include <sstream>
@@ -45,6 +45,7 @@ DEFINE_string(param, "", "Load XML from a ROS param.");
 DEFINE_string(string, "", "Load XML from a string.");
 DEFINE_string(topic, "", "Load XML from a ROS string publisher.");
 DEFINE_string(name, "", "Name for spawned entity.");
+DEFINE_string(ns, "", "Namespace for spawned entity.");
 DEFINE_bool(allow_renaming, false, "Rename entity if name already used.");
 DEFINE_double(x, 0, "X component of initial position, in meters.");
 DEFINE_double(y, 0, "Y component of initial position, in meters.");
@@ -57,7 +58,7 @@ DEFINE_double(Y, 0, "Yaw component of initial orientation, in radians.");
 
 bool set_XML_from_topic(
   const std::string & topic_name, const rclcpp::Node::SharedPtr ros2_node,
-  gz::msgs::EntityFactory & req)
+  gz::msgs::EntityFactoryWithNs & req)
 {
   const auto timeout = std::chrono::seconds(1);
   std::promise<std::string> xml_promise;
@@ -112,7 +113,7 @@ int main(int _argc, char ** _argv)
   gflags::AllowCommandLineReparsing();
   gflags::SetUsageMessage(
     R"(Usage: create -world [arg] [-file FILE] [-param PARAM] [-topic TOPIC]
-                       [-string STRING] [-name NAME] [-allow_renaming RENAMING] [-x X] [-y Y] [-z Z]
+                       [-string STRING] [-name NAME] [-ns NAMESPACE] [-allow_renaming RENAMING] [-x X] [-y Y] [-z Z]
                        [-R ROLL] [-P PITCH] [-Y YAW])");
   gflags::ParseCommandLineFlags(&filtered_argc, &filtered_argv, false);
 
@@ -128,6 +129,7 @@ int main(int _argc, char ** _argv)
   ros2_node->declare_parameter("string", "");
   ros2_node->declare_parameter("topic", "");
   ros2_node->declare_parameter("name", "");
+  ros2_node->declare_parameter("ns", "");
   ros2_node->declare_parameter("allow_renaming", false);
   ros2_node->declare_parameter("x", static_cast<double>(0));
   ros2_node->declare_parameter("y", static_cast<double>(0));
@@ -176,10 +178,11 @@ int main(int _argc, char ** _argv)
       "World name was not provided. Using [%s] as the default world.",
       world_name.c_str());
   }
-  std::string service{"/world/" + world_name + "/create"};
+
+  std::string service{"/world/" + world_name + "/create_with_ns"};
 
   // Request message
-  gz::msgs::EntityFactory req;
+  gz::msgs::EntityFactoryWithNs req;
 
   // Get ROS parameters
   std::string file_name = ros2_node->get_parameter("file").as_string();
@@ -255,6 +258,14 @@ int main(int _argc, char ** _argv)
     req.set_name(FLAGS_name);
   }
 
+  // Namespace
+  std::string ns = ros2_node->get_parameter("ns").as_string();
+  if (!ns.empty()) {
+    req.set_entity_namespace(ns);
+  } else if (!FLAGS_ns.empty()) {
+    req.set_entity_namespace(FLAGS_ns);
+  }
+
   // Allow Renaming
   bool allow_renaming = ros2_node->get_parameter("allow_renaming").as_bool();
   req.set_allow_renaming((allow_renaming || FLAGS_allow_renaming));

ros_gz_sim/src/gz_simulation_interfaces/services/spawn_entity.cpp

@@ -15,7 +15,7 @@
 #include "spawn_entity.hpp"
 
 #include <gz/msgs/boolean.pb.h>
-#include <gz/msgs/entity_factory.pb.h>
+#include <gz/msgs/entity_factory_with_ns.pb.h>
 
 #include <memory>
 
@@ -36,7 +36,7 @@ SpawnEntity::SpawnEntity(
   std::shared_ptr<rclcpp::Node> ros_node, std::shared_ptr<GazeboProxy> gz_proxy)
 : HandlerBase(ros_node, gz_proxy)
 {
-  const auto create_service = this->gz_proxy_->PrefixTopic("create/blocking");
+  const auto create_service = this->gz_proxy_->PrefixTopic("create_with_ns/blocking");
   if (!this->gz_proxy_->WaitForGzService(create_service)) {
     RCLCPP_ERROR_STREAM(
       this->ros_node_->get_logger(),
@@ -47,7 +47,7 @@ SpawnEntity::SpawnEntity(
   this->services_handle_ = ros_node->create_service<SpawnEntitySrv>(
     "spawn_entity", [this, create_service](RequestPtr request, ResponsePtr response) {
       using Result = simulation_interfaces::msg::Result;
-      gz::msgs::EntityFactory gz_request;
+      gz::msgs::EntityFactoryWithNs gz_request;
       if (!request->name.empty()) {
         gz_request.set_name(request->name);
       }
@@ -67,7 +67,8 @@ SpawnEntity::SpawnEntity(
         return;
       }
 
-      // TODO(azeey) Add support for entity_namespace
+      gz_request.set_entity_namespace(request->entity_namespace);
+
       // TODO(azeey) Reuse code in ros_gz_bridge/convert/geometry_msgs
       auto * pose = gz_request.mutable_pose();
       pose->mutable_position()->set_x(request->initial_pose.pose.position.x);

ros_gz_sim/test/test_create.cpp

@@ -14,15 +14,15 @@
 
 
 #include <gz/msgs/boolean.pb.h>
-#include <gz/msgs/entity_factory.pb.h>
+#include <gz/msgs/entity_factory_with_ns.pb.h>
 #include <csignal>
 #include <chrono>
 #include <condition_variable>
 #include <functional>
 #include <mutex>
 #include <gz/transport/Node.hh>
 
-// Simple application that provides a `/create` service and prints out the
+// Simple application that provides a `/create_with_ns` service and prints out the
 // sdf_filename of the request. This works in conjection with
 // test_create_node.launch.py
 int main()
@@ -34,7 +34,7 @@ int main()
   gz::transport::Node node;
   auto cb = std::function(
     [&](
-      const gz::msgs::EntityFactory & _req,
+      const gz::msgs::EntityFactoryWithNs & _req,
       gz::msgs::Boolean & _res) -> bool {
       std::cout << _req.sdf_filename() << std::endl;
       _res.set_data(true);
@@ -47,7 +47,7 @@ int main()
       return true;
     });
 
-  node.Advertise("/world/default/create", cb);
+  node.Advertise("/world/default/create_with_ns", cb);
   // wait until we receive a message.
   std::unique_lock<std::mutex> lk(m);
   cv.wait(lk, [&] {return test_complete;});