extend tilde (~) substitution with node namespace to full prefix

Author: kuralme

controller_interface/include/controller_interface/helpers.hpp

@@ -108,7 +108,12 @@ inline std::string resolve_tf_prefix(const std::string & prefix, const std::stri
     return std::string{};
   }
 
-  std::string nprefix = prefix == "~" ? node_ns : prefix;
+  std::string nprefix = prefix;
+  std::size_t pos = nprefix.find("~");
+  if (pos != std::string::npos)
+  {
+    nprefix.replace(pos, 1, node_ns);
+  }
 
   // ensure trailing '/'
   if (nprefix.back() != '/')

controller_interface/test/test_controller_tf_prefix.cpp

@@ -22,12 +22,6 @@ TEST_F(TestControllerTFPrefix, EmptyPrefixReturnsEmpty)
   EXPECT_EQ(controller_interface::resolve_tf_prefix("", "/ns"), "");
 }
 
-TEST_F(TestControllerTFPrefix, TildePrefixUsesNamespace)
-{
-  EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns"), "ns/");
-  EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns/"), "ns/");
-}
-
 TEST_F(TestControllerTFPrefix, ExplicitPrefixUsed)
 {
   EXPECT_EQ(controller_interface::resolve_tf_prefix("robot", "/ns"), "robot/");
@@ -40,3 +34,11 @@ TEST_F(TestControllerTFPrefix, NormalizePrefixSlashes)
   EXPECT_EQ(controller_interface::resolve_tf_prefix("/robot3/", "/ns"), "robot3/");
   EXPECT_EQ(controller_interface::resolve_tf_prefix("/", "/ns"), "");
 }
+
+TEST_F(TestControllerTFPrefix, TildePrefixResolvesToNamespace)
+{
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~", "/ns"), "ns/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~/", "/ns"), "ns/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("~/robot", "/ns"), "ns/robot/");
+  EXPECT_EQ(controller_interface::resolve_tf_prefix("/~/robot/", "ns"), "ns/robot/");
+}