Sub Node alternative (#581)

* Sub Node alternative

* Sub Node alternative

* Test // characters in namespaces

* Sub Node alternative

* Test // characters in namespaces

* Fixing style and warning in the order of initalizing members

* Fixing cases with / in different positions, and adding new tests

* Removing commented methods

* Changing extended_namespace to sub_namespace

* Fixed a bug when merging

* Fixed a bug when merging

* Sub Node alternative

* Sub Node alternative

* Test // characters in namespaces

* Fixing style and warning in the order of initalizing members

* Fixing cases with / in different positions, and adding new tests

* Removing commented methods

* Changing extended_namespace to sub_namespace

* Fixed a bug when merging

* Merge with origin to update branch

* improvements to API and documentation

Signed-off-by: William Woodall <william@osrfoundation.org>

* style and fixing tests

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup subnode specific tests

Signed-off-by: William Woodall <william@osrfoundation.org>

* remove vestigial function

Signed-off-by: William Woodall <william@osrfoundation.org>

* improve documentation

Signed-off-by: William Woodall <william@osrfoundation.org>

* add test to check interaction between ~ and sub-nodes

Signed-off-by: William Woodall <william@osrfoundation.org>

* typo

Signed-off-by: William Woodall <william@osrfoundation.org>

rclcpp/include/rclcpp/node.hpp

@@ -102,7 +102,15 @@ public:
   get_name() const;
 
   /// Get the namespace of the node.
-  /** \return The namespace of the node. */
+  /**
+   * This namespace is the "node's" namespace, and therefore is not affected
+   * by any sub-namespace's that may affect entities created with this instance.
+   * Use get_effective_namespace() to get the full namespace used by entities.
+   *
+   * \sa get_sub_namespace()
+   * \sa get_effective_namespace()
+   * \return The namespace of the node.
+   */
   RCLCPP_PUBLIC
   const char *
   get_namespace() const;
@@ -481,6 +489,122 @@ public:
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr
   get_node_time_source_interface();
 
+  /// Return the sub-namespace, if this is a sub-node, otherwise an empty string.
+  /**
+   * The returned sub-namespace is either the accumulated sub-namespaces which
+   * were given to one-to-many create_sub_node() calls, or an empty string if
+   * this is an original node instance, i.e. not a sub-node.
+   *
+   * For example, consider:
+   *
+   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   *   node->get_sub_namespace();  // -> ""
+   *   auto sub_node1 = node->create_sub_node("a");
+   *   sub_node1->get_sub_namespace();  // -> "a"
+   *   auto sub_node2 = sub_node1->create_sub_node("b");
+   *   sub_node2->get_sub_namespace();  // -> "a/b"
+   *   auto sub_node3 = node->create_sub_node("foo");
+   *   sub_node3->get_sub_namespace();  // -> "foo"
+   *   node->get_sub_namespace();  // -> ""
+   *
+   * get_namespace() will return the original node namespace, and will not
+   * include the sub-namespace if one exists.
+   * To get that you need to call the get_effective_namespace() method.
+   *
+   * \sa get_namespace()
+   * \sa get_effective_namespace()
+   * \return the sub-namespace string, not including the node's original namespace
+   */
+  RCLCPP_PUBLIC
+  const std::string &
+  get_sub_namespace() const;
+
+  /// Return the effective namespace that is used when creating entities.
+  /**
+   * The returned namespace is a concatenation of the node namespace and the
+   * accumulated sub-namespaces, which is used as the namespace when creating
+   * entities which have relative names.
+   *
+   * For example, consider:
+   *
+   *   auto node = std::make_shared<rclcpp::Node>("my_node", "my_ns");
+   *   node->get_effective_namespace();  // -> "/my_ns"
+   *   auto sub_node1 = node->create_sub_node("a");
+   *   sub_node1->get_effective_namespace();  // -> "/my_ns/a"
+   *   auto sub_node2 = sub_node1->create_sub_node("b");
+   *   sub_node2->get_effective_namespace();  // -> "/my_ns/a/b"
+   *   auto sub_node3 = node->create_sub_node("foo");
+   *   sub_node3->get_effective_namespace();  // -> "/my_ns/foo"
+   *   node->get_effective_namespace();  // -> "/my_ns"
+   *
+   * \sa get_namespace()
+   * \sa get_sub_namespace()
+   * \return the sub-namespace string, not including the node's original namespace
+   */
+  RCLCPP_PUBLIC
+  const std::string &
+  get_effective_namespace() const;
+
+  /// Create a sub-node, which will extend the namespace of all entities created with it.
+  /**
+   * A sub-node (short for subordinate node) is an instance of this class
+   * which has been created using an existing instance of this class, but which
+   * has an additional sub-namespace (short for subordinate namespace)
+   * associated with it.
+   * The sub-namespace will extend the node's namespace for the purpose of
+   * creating additional entities, such as Publishers, Subscriptions, Service
+   * Clients and Servers, and so on.
+   *
+   * By default, when an instance of this class is created using one of the
+   * public constructors, it has no sub-namespace associated with it, and
+   * therefore is not a sub-node.
+   * That "normal" node instance may, however, be used to create further
+   * instances of this class, based on the original instance, which have an
+   * additional sub-namespace associated with them.
+   * This may be done by using this method, create_sub_node().
+   *
+   * Furthermore, a sub-node may be used to create additional sub-node's, in
+   * which case the sub-namespace passed to this function will further
+   * extend the sub-namespace of the existing sub-node.
+   * See get_sub_namespace() and get_effective_namespace() for examples.
+   *
+   * Note that entities which use absolute names are not affected by any
+   * namespaces, neither the normal node namespace nor any sub-namespace.
+   * Note also that the fully qualified node name is unaffected by a
+   * sub-namespace.
+   *
+   * The sub-namespace should be relative, and an exception will be thrown if
+   * the sub-namespace is absolute, i.e. if it starts with a leading '/'.
+   *
+   * \sa get_sub_namespace()
+   * \sa get_effective_namespace()
+   * \param[in] sub_namespace sub-namespace of the sub-node.
+   * \return newly created sub-node
+   * \throws NameValidationError if the sub-namespace is absolute, i.e. starts
+   *   with a leading '/'.
+   */
+  RCLCPP_PUBLIC
+  Node::SharedPtr
+  create_sub_node(const std::string & sub_namespace);
+
+  /// Return the NodeOptions used when creating this node.
+  RCLCPP_PUBLIC
+  const NodeOptions &
+  get_node_options() const;
+
+protected:
+  /// Construct a sub-node, which will extend the namespace of all entities created with it.
+  /**
+   * \sa create_sub_node()
+   *
+   * \param[in] other The node from which a new sub-node is created.
+   * \param[in] sub_namespace The sub-namespace of the sub-node.
+   */
+  RCLCPP_PUBLIC
+  Node(
+    const Node & other,
+    const std::string & sub_namespace);
+
 private:
   RCLCPP_DISABLE_COPY(Node)
 
@@ -499,7 +623,9 @@ private:
   rclcpp::node_interfaces::NodeTimeSourceInterface::SharedPtr node_time_source_;
   rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
-  bool use_intra_process_comms_;
+  const NodeOptions node_options_;
+  const std::string sub_namespace_;
+  const std::string effective_namespace_;
 };
 
 }  // namespace rclcpp

rclcpp/include/rclcpp/node_impl.hpp

@@ -65,6 +65,18 @@ Node::create_publisher(
   return this->create_publisher<MessageT, Alloc, PublisherT>(topic_name, qos, allocator);
 }
 
+RCLCPP_LOCAL
+inline
+std::string
+extend_name_with_sub_namespace(const std::string & name, const std::string & sub_namespace)
+{
+  std::string name_with_sub_namespace(name);
+  if (sub_namespace != "" && name.front() != '/' && name.front() != '~') {
+    name_with_sub_namespace = sub_namespace + "/" + name;
+  }
+  return name_with_sub_namespace;
+}
+
 template<typename MessageT, typename Alloc, typename PublisherT>
 std::shared_ptr<PublisherT>
 Node::create_publisher(
@@ -74,11 +86,12 @@ Node::create_publisher(
   if (!allocator) {
     allocator = std::make_shared<Alloc>();
   }
+
   return rclcpp::create_publisher<MessageT, Alloc, PublisherT>(
     this->node_topics_.get(),
-    topic_name,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
     qos_profile,
-    use_intra_process_comms_,
+    this->get_node_options().use_intra_process_comms(),
     allocator);
 }
 
@@ -112,12 +125,12 @@ Node::create_subscription(
 
   return rclcpp::create_subscription<MessageT, CallbackT, Alloc, CallbackMessageT, SubscriptionT>(
     this->node_topics_.get(),
-    topic_name,
+    extend_name_with_sub_namespace(topic_name, this->get_sub_namespace()),
     std::forward<CallbackT>(callback),
     qos_profile,
     group,
     ignore_local_publications,
-    use_intra_process_comms_,
+    this->get_node_options().use_intra_process_comms(),
     msg_mem_strat,
     allocator);
 }
@@ -141,6 +154,7 @@ Node::create_subscription(
 {
   rmw_qos_profile_t qos = rmw_qos_profile_default;
   qos.depth = qos_history_depth;
+
   return this->create_subscription<MessageT>(
     topic_name,
     std::forward<CallbackT>(callback),
@@ -182,7 +196,7 @@ Node::create_client(
   auto cli = Client<ServiceT>::make_shared(
     node_base_.get(),
     node_graph_,
-    service_name,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
     options);
 
   auto cli_base_ptr = std::dynamic_pointer_cast<ClientBase>(cli);
@@ -199,8 +213,12 @@ Node::create_service(
   rclcpp::callback_group::CallbackGroup::SharedPtr group)
 {
   return rclcpp::create_service<ServiceT, CallbackT>(
-    node_base_, node_services_,
+    node_base_,
-    service_name, std::forward<CallbackT>(callback), qos_profile, group);
+    node_services_,
+    extend_name_with_sub_namespace(service_name, this->get_sub_namespace()),
+    std::forward<CallbackT>(callback),
+    qos_profile,
+    group);
 }
 
 template<typename CallbackT>
@@ -216,10 +234,13 @@ Node::set_parameter_if_not_set(
   const std::string & name,
   const ParameterT & value)
 {
+  std::string parameter_name_with_sub_namespace =
+    extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
   rclcpp::Parameter parameter;
-  if (!this->get_parameter(name, parameter)) {
+  if (!this->get_parameter(parameter_name_with_sub_namespace, parameter)) {
     this->set_parameters({
-        rclcpp::Parameter(name, value),
+        rclcpp::Parameter(parameter_name_with_sub_namespace, value),
       });
   }
 }
@@ -250,8 +271,11 @@ template<typename ParameterT>
 bool
 Node::get_parameter(const std::string & name, ParameterT & value) const
 {
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
   rclcpp::Parameter parameter;
-  bool result = get_parameter(name, parameter);
+
+  bool result = get_parameter(sub_name, parameter);
   if (result) {
     value = parameter.get_value<ParameterT>();
   }
@@ -286,7 +310,9 @@ Node::get_parameter_or(
   ParameterT & value,
   const ParameterT & alternative_value) const
 {
-  bool got_parameter = get_parameter(name, value);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, value);
   if (!got_parameter) {
     value = alternative_value;
   }
@@ -300,10 +326,12 @@ Node::get_parameter_or_set(
   ParameterT & value,
   const ParameterT & alternative_value)
 {
-  bool got_parameter = get_parameter(name, value);
+  std::string sub_name = extend_name_with_sub_namespace(name, this->get_sub_namespace());
+
+  bool got_parameter = get_parameter(sub_name, value);
   if (!got_parameter) {
     this->set_parameters({
-        rclcpp::Parameter(name, alternative_value),
+        rclcpp::Parameter(sub_name, alternative_value),
       });
     value = alternative_value;
   }

rclcpp/src/rclcpp/node.cpp

@@ -34,10 +34,59 @@
 #include "rclcpp/node_interfaces/node_topics.hpp"
 #include "rclcpp/node_interfaces/node_waitables.hpp"
 
+#include "rmw/validate_namespace.h"
+
 using rclcpp::Node;
 using rclcpp::NodeOptions;
 using rclcpp::exceptions::throw_from_rcl_error;
 
+RCLCPP_LOCAL
+std::string
+extend_sub_namespace(const std::string & existing_sub_namespace, const std::string & extension)
+{
+  // Assumption is that the existing_sub_namespace does not need checking
+  // because it would be checked already when it was set with this function.
+
+  // check if the new sub-namespace extension is absolute
+  if (extension.front() == '/') {
+    throw rclcpp::exceptions::NameValidationError(
+            "sub_namespace",
+            extension.c_str(),
+            "a sub-namespace should not have a leading /",
+            0);
+  }
+
+  std::string new_sub_namespace;
+  if (existing_sub_namespace.empty()) {
+    new_sub_namespace = extension;
+  } else {
+    new_sub_namespace = existing_sub_namespace + "/" + extension;
+  }
+
+  // remove any trailing `/` so that new extensions do no result in `//`
+  if (new_sub_namespace.back() == '/') {
+    new_sub_namespace = new_sub_namespace.substr(0, new_sub_namespace.size() - 1);
+  }
+
+  return new_sub_namespace;
+}
+
+RCLCPP_LOCAL
+std::string
+create_effective_namespace(const std::string & node_namespace, const std::string & sub_namespace)
+{
+  // Assumption is that both the node_namespace and sub_namespace are conforming
+  // and do not need trimming of `/` and other things, as they were validated
+  // in other functions already.
+
+  if (node_namespace.back() == '/') {
+    // this is the special case where node_namespace is just `/`
+    return node_namespace + sub_namespace;
+  } else {
+    return node_namespace + "/" + sub_namespace;
+  }
+}
+
 Node::Node(
   const std::string & node_name,
   const NodeOptions & options)
@@ -84,10 +133,48 @@ Node::Node(
       node_parameters_
     )),
   node_waitables_(new rclcpp::node_interfaces::NodeWaitables(node_base_.get())),
-  use_intra_process_comms_(options.use_intra_process_comms())
+  node_options_(options),
+  sub_namespace_(""),
+  effective_namespace_(create_effective_namespace(this->get_namespace(), sub_namespace_))
 {
 }
 
+Node::Node(
+  const Node & other,
+  const std::string & sub_namespace)
+: node_base_(other.node_base_),
+  node_graph_(other.node_graph_),
+  node_logging_(other.node_logging_),
+  node_timers_(other.node_timers_),
+  node_topics_(other.node_topics_),
+  node_services_(other.node_services_),
+  node_clock_(other.node_clock_),
+  node_parameters_(other.node_parameters_),
+  node_options_(other.node_options_),
+  sub_namespace_(extend_sub_namespace(other.get_sub_namespace(), sub_namespace)),
+  effective_namespace_(create_effective_namespace(other.get_namespace(), sub_namespace_))
+{
+  // Validate new effective namespace.
+  int validation_result;
+  size_t invalid_index;
+  rmw_ret_t rmw_ret =
+    rmw_validate_namespace(effective_namespace_.c_str(), &validation_result, &invalid_index);
+
+  if (rmw_ret != RMW_RET_OK) {
+    if (rmw_ret == RMW_RET_INVALID_ARGUMENT) {
+      throw_from_rcl_error(RCL_RET_INVALID_ARGUMENT, "failed to validate subnode namespace");
+    }
+    throw_from_rcl_error(RCL_RET_ERROR, "failed to validate subnode namespace");
+  }
+
+  if (validation_result != RMW_NAMESPACE_VALID) {
+    throw rclcpp::exceptions::InvalidNamespaceError(
+            effective_namespace_.c_str(),
+            rmw_namespace_validation_result_string(validation_result),
+            invalid_index);
+  }
+}
+
 Node::~Node()
 {}
 
@@ -298,3 +385,29 @@ Node::get_node_waitables_interface()
 {
   return node_waitables_;
 }
+
+const std::string &
+Node::get_sub_namespace() const
+{
+  return this->sub_namespace_;
+}
+
+const std::string &
+Node::get_effective_namespace() const
+{
+  return this->effective_namespace_;
+}
+
+Node::SharedPtr
+Node::create_sub_node(const std::string & sub_namespace)
+{
+  // Cannot use make_shared<Node>() here as it requires the constructor to be
+  // public, and this constructor is intentionally protected instead.
+  return std::shared_ptr<Node>(new Node(*this, sub_namespace));
+}
+
+const NodeOptions &
+Node::get_node_options() const
+{
+  return this->node_options_;
+}

rclcpp/test/test_client.cpp

@@ -43,6 +43,28 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestClientSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 /*
    Testing client construction and destruction.
  */
@@ -58,3 +80,20 @@ TEST_F(TestClient, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+/*
+   Testing client construction and destruction for subnodes.
+ */
+TEST_F(TestClientSub, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  {
+    auto client = subnode->create_client<ListParameters>("service");
+    EXPECT_STREQ(client->get_service_name(), "/ns/sub_ns/service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto client = node->create_client<ListParameters>("invalid_service?");
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_node.cpp

@@ -75,6 +75,129 @@ TEST_F(TestNode, get_name_and_namespace) {
   }
 }
 
+TEST_F(TestNode, subnode_get_name_and_namespace) {
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+    auto subnode = node->create_sub_node("sub_ns");
+    EXPECT_STREQ("my_node", subnode->get_name());
+    EXPECT_STREQ("/ns", subnode->get_namespace());
+    EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str());
+    EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    auto subnode = node->create_sub_node("sub_ns");
+    EXPECT_STREQ("my_node", subnode->get_name());
+    EXPECT_STREQ("/ns", subnode->get_namespace());
+    EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str());
+    EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node");
+    auto subnode = node->create_sub_node("sub_ns");
+    EXPECT_STREQ("my_node", subnode->get_name());
+    EXPECT_STREQ("/", subnode->get_namespace());
+    EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str());
+    EXPECT_STREQ("/sub_ns", subnode->get_effective_namespace().c_str());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    auto subnode = node->create_sub_node("sub_ns");
+    EXPECT_STREQ("my_node", subnode->get_name());
+    EXPECT_STREQ("/ns", subnode->get_namespace());
+    EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str());
+    EXPECT_STREQ("/ns/sub_ns", subnode->get_effective_namespace().c_str());
+    auto subnode2 = subnode->create_sub_node("sub_ns2");
+    EXPECT_STREQ("my_node", subnode2->get_name());
+    EXPECT_STREQ("/ns", subnode2->get_namespace());
+    EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str());
+    EXPECT_STREQ("/ns/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node");
+    auto subnode = node->create_sub_node("sub_ns");
+    EXPECT_STREQ("my_node", subnode->get_name());
+    EXPECT_STREQ("/", subnode->get_namespace());
+    EXPECT_STREQ("sub_ns", subnode->get_sub_namespace().c_str());
+    EXPECT_STREQ("/sub_ns", subnode->get_effective_namespace().c_str());
+    auto subnode2 = subnode->create_sub_node("sub_ns2");
+    EXPECT_STREQ("my_node", subnode2->get_name());
+    EXPECT_STREQ("/", subnode2->get_namespace());
+    EXPECT_STREQ("sub_ns/sub_ns2", subnode2->get_sub_namespace().c_str());
+    EXPECT_STREQ("/sub_ns/sub_ns2", subnode2->get_effective_namespace().c_str());
+  }
+  {
+    auto node = std::make_shared<rclcpp::Node>("my_node");
+    ASSERT_THROW({
+      auto subnode = node->create_sub_node("/sub_ns");
+    }, rclcpp::exceptions::NameValidationError);
+  }
+}
+/*
+   Testing node construction and destruction.
+ */
+TEST_F(TestNode, subnode_construction_and_destruction) {
+  {
+    ASSERT_NO_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+      auto subnode = node->create_sub_node("sub_ns");
+    });
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+      auto subnode = node->create_sub_node("invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns/");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns/");
+      auto subnode = node->create_sub_node("/sub_ns");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+      auto subnode = node->create_sub_node("/sub_ns");
+    }, rclcpp::exceptions::NameValidationError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "ns");
+      auto subnode = node->create_sub_node("~sub_ns");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("invalid_ns?");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+  {
+    ASSERT_NO_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("sub_ns");
+    });
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("/sub_ns");
+    }, rclcpp::exceptions::NameValidationError);
+  }
+  {
+    ASSERT_THROW({
+      auto node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+      auto subnode = node->create_sub_node("~sub_ns");
+    }, rclcpp::exceptions::InvalidNamespaceError);
+  }
+}
+
 TEST_F(TestNode, get_logger) {
   {
     auto node = std::make_shared<rclcpp::Node>("my_node");

rclcpp/test/test_publisher.cpp

@@ -43,6 +43,28 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestPublisherSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 /*
    Testing publisher construction and destruction.
  */
@@ -58,3 +80,27 @@ TEST_F(TestPublisher, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidTopicNameError);
   }
 }
+
+/*
+   Testing publisher construction and destruction for subnodes.
+ */
+TEST_F(TestPublisherSub, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  {
+    auto publisher = subnode->create_publisher<IntraProcessMessage>("topic");
+
+    EXPECT_STREQ(publisher->get_topic_name(), "/ns/sub_ns/topic");
+  }
+
+  {
+    auto publisher = subnode->create_publisher<IntraProcessMessage>("/topic");
+
+    EXPECT_STREQ(publisher->get_topic_name(), "/topic");
+  }
+
+  {
+    ASSERT_THROW({
+      auto publisher = subnode->create_publisher<IntraProcessMessage>("invalid_topic?");
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}

rclcpp/test/test_service.cpp

@@ -43,6 +43,28 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestServiceSub : public ::testing::Test
+{
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("my_node", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 /*
    Testing service construction and destruction.
  */
@@ -61,3 +83,23 @@ TEST_F(TestService, construction_and_destruction) {
     }, rclcpp::exceptions::InvalidServiceNameError);
   }
 }
+
+/*
+   Testing service construction and destruction for subnodes.
+ */
+TEST_F(TestServiceSub, construction_and_destruction) {
+  using rcl_interfaces::srv::ListParameters;
+  auto callback =
+    [](const ListParameters::Request::SharedPtr, ListParameters::Response::SharedPtr) {
+    };
+  {
+    auto service = subnode->create_service<ListParameters>("service", callback);
+    EXPECT_STREQ(service->get_service_name(), "/ns/sub_ns/service");
+  }
+
+  {
+    ASSERT_THROW({
+      auto service = node->create_service<ListParameters>("invalid_service?", callback);
+    }, rclcpp::exceptions::InvalidServiceNameError);
+  }
+}

rclcpp/test/test_subscription.cpp

@@ -49,6 +49,34 @@ protected:
   rclcpp::Node::SharedPtr node;
 };
 
+class TestSubscriptionSub : public ::testing::Test
+{
+public:
+  void OnMessage(const rcl_interfaces::msg::IntraProcessMessage::SharedPtr msg)
+  {
+    (void)msg;
+  }
+
+protected:
+  static void SetUpTestCase()
+  {
+  }
+
+  void SetUp()
+  {
+    node = std::make_shared<rclcpp::Node>("test_subscription", "/ns");
+    subnode = node->create_sub_node("sub_ns");
+  }
+
+  void TearDown()
+  {
+    node.reset();
+  }
+
+  rclcpp::Node::SharedPtr node;
+  rclcpp::Node::SharedPtr subnode;
+};
+
 class SubscriptionClassNodeInheritance : public rclcpp::Node
 {
 public:
@@ -107,6 +135,38 @@ TEST_F(TestSubscription, construction_and_destruction) {
   }
 }
 
+/*
+   Testing subscription construction and destruction for subnodes.
+ */
+TEST_F(TestSubscriptionSub, construction_and_destruction) {
+  using rcl_interfaces::msg::IntraProcessMessage;
+  auto callback = [](const IntraProcessMessage::SharedPtr msg) {
+      (void)msg;
+    };
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("topic", callback);
+    EXPECT_STREQ(sub->get_topic_name(), "/ns/sub_ns/topic");
+  }
+
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("/topic", callback);
+    EXPECT_STREQ(sub->get_topic_name(), "/topic");
+  }
+
+  {
+    auto sub = subnode->create_subscription<IntraProcessMessage>("~/topic", callback);
+    std::string expected_topic_name =
+      std::string(node->get_namespace()) + "/" + node->get_name() + "/topic";
+    EXPECT_STREQ(sub->get_topic_name(), expected_topic_name.c_str());
+  }
+
+  {
+    ASSERT_THROW({
+      auto sub = node->create_subscription<IntraProcessMessage>("invalid_topic?", callback);
+    }, rclcpp::exceptions::InvalidTopicNameError);
+  }
+}
+
 /*
    Testing subscriptions using std::bind.
  */