Support arbitrary message namespaces (#266)

* Support arbitrary message namespaces

Message type namespaces are no longer restricted to 'msg' and 'srv'.

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Update demangling logic for handling arbitrary ROS namespaces

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Rename struct member

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Update assertion

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Support arbitrary message namespace in rmw_fastrtps_dynamic_cpp

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Combine package name with message namespace

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Fix bugs

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* Address review

Signed-off-by: Jacob Perron <jacob@openrobotics.org>

* fix style

Signed-off-by: Dirk Thomas <dirk-thomas@users.noreply.github.com>

rmw_fastrtps_cpp/src/rmw_client.cpp

@@ -105,8 +105,8 @@ rmw_create_client(
   response_members = static_cast<const message_type_support_callbacks_t *>(
     service_members->response_members_->data);
 
-  std::string request_type_name = _create_type_name(request_members, "srv");
-  std::string response_type_name = _create_type_name(response_members, "srv");
+  std::string request_type_name = _create_type_name(request_members);
+  std::string response_type_name = _create_type_name(response_members);
 
   if (!Domain::getRegisteredType(participant, request_type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->request_type_support_)))

rmw_fastrtps_cpp/src/rmw_publisher.cpp

@@ -128,7 +128,7 @@ rmw_create_publisher(
   info->typesupport_identifier_ = type_support->typesupport_identifier;
 
   auto callbacks = static_cast<const message_type_support_callbacks_t *>(type_support->data);
-  std::string type_name = _create_type_name(callbacks, "msg");
+  std::string type_name = _create_type_name(callbacks);
   if (!Domain::getRegisteredType(participant, type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->type_support_)))
   {

rmw_fastrtps_cpp/src/rmw_service.cpp

@@ -115,8 +115,8 @@ rmw_create_service(
   response_members = static_cast<const message_type_support_callbacks_t *>(
     service_members->response_members_->data);
 
-  std::string request_type_name = _create_type_name(request_members, "srv");
-  std::string response_type_name = _create_type_name(response_members, "srv");
+  std::string request_type_name = _create_type_name(request_members);
+  std::string response_type_name = _create_type_name(response_members);
 
   if (!Domain::getRegisteredType(participant, request_type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->request_type_support_)))

rmw_fastrtps_cpp/src/rmw_subscription.cpp

@@ -131,7 +131,7 @@ rmw_create_subscription(
   info->typesupport_identifier_ = type_support->typesupport_identifier;
 
   auto callbacks = static_cast<const message_type_support_callbacks_t *>(type_support->data);
-  std::string type_name = _create_type_name(callbacks, "msg");
+  std::string type_name = _create_type_name(callbacks);
   if (!Domain::getRegisteredType(participant, type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->type_support_)))
   {

rmw_fastrtps_cpp/src/type_support_common.cpp

@@ -100,8 +100,7 @@ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t *
 {
   assert(members);
 
-  std::string name = std::string(members->package_name_) + "::msg::dds_::" +
-    members->message_name_ + "_";
+  std::string name = _create_type_name(members);
   this->setName(name.c_str());
 
   set_members(members);
@@ -115,25 +114,23 @@ RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t *
 {
   assert(members);
 
-  std::string name = std::string(members->package_name_) + "::srv::dds_::" +
-    members->service_name_ + "_Request_";
-  this->setName(name.c_str());
-
   auto msg = static_cast<const message_type_support_callbacks_t *>(
     members->request_members_->data);
+  std::string name = _create_type_name(msg);  // + "Request_";
+  this->setName(name.c_str());
+
   set_members(msg);
 }
 
 ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members)
 {
   assert(members);
 
-  std::string name = std::string(members->package_name_) + "::srv::dds_::" +
-    members->service_name_ + "_Response_";
-  this->setName(name.c_str());
-
   auto msg = static_cast<const message_type_support_callbacks_t *>(
     members->response_members_->data);
+  std::string name = _create_type_name(msg);  // + "Response_";
+  this->setName(name.c_str());
+
   set_members(msg);
 }
 

rmw_fastrtps_cpp/src/type_support_common.hpp

@@ -15,6 +15,7 @@
 #ifndef TYPE_SUPPORT_COMMON_HPP_
 #define TYPE_SUPPORT_COMMON_HPP_
 
+#include <sstream>
 #include <string>
 
 #include "fastrtps/Domain.h"
@@ -43,15 +44,21 @@ using ResponseTypeSupport_cpp = rmw_fastrtps_cpp::ResponseTypeSupport;
 
 inline std::string
 _create_type_name(
-  const message_type_support_callbacks_t * members,
-  const std::string & sep)
+  const message_type_support_callbacks_t * members)
 {
   if (!members) {
     RMW_SET_ERROR_MSG("members handle is null");
     return "";
   }
-  return
-    std::string(members->package_name_) + "::" + sep + "::dds_::" + members->message_name_ + "_";
+
+  std::ostringstream ss;
+  std::string message_namespace(members->message_namespace_);
+  std::string message_name(members->message_name_);
+  if (!message_namespace.empty()) {
+    ss << message_namespace << "::";
+  }
+  ss << "dds_::" << message_name << "_";
+  return ss.str();
 }
 
 inline void

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/MessageTypeSupport_impl.hpp

@@ -20,6 +20,8 @@
 
 #include <cassert>
 #include <memory>
+#include <regex>
+#include <sstream>
 #include <string>
 
 #include "rmw_fastrtps_dynamic_cpp/MessageTypeSupport.hpp"
@@ -34,9 +36,16 @@ MessageTypeSupport<MembersType>::MessageTypeSupport(const MembersType * members)
   assert(members);
   this->members_ = members;
 
-  std::string name = std::string(members->package_name_) + "::msg::dds_::" +
-    members->message_name_ + "_";
-  this->setName(name.c_str());
+  std::ostringstream ss;
+  std::string message_namespace(this->members_->message_namespace_);
+  std::string message_name(this->members_->message_name_);
+  if (!message_namespace.empty()) {
+    // Find and replace C namespace separator with C++, in case this is using C typesupport
+    message_namespace = std::regex_replace(message_namespace, std::regex("__"), "::");
+    ss << message_namespace << "::";
+  }
+  ss << "dds_::" << message_name << "_";
+  this->setName(ss.str().c_str());
 
   // Fully bound by default
   this->max_size_bound_ = true;

rmw_fastrtps_dynamic_cpp/include/rmw_fastrtps_dynamic_cpp/ServiceTypeSupport_impl.hpp

@@ -18,6 +18,8 @@
 #include <fastcdr/FastBuffer.h>
 #include <fastcdr/Cdr.h>
 #include <cassert>
+#include <regex>
+#include <sstream>
 #include <string>
 
 #include "rmw_fastrtps_dynamic_cpp/ServiceTypeSupport.hpp"
@@ -38,9 +40,16 @@ RequestTypeSupport<ServiceMembersType, MessageMembersType>::RequestTypeSupport(
   assert(members);
   this->members_ = members->request_members_;
 
-  std::string name = std::string(members->package_name_) + "::srv::dds_::" +
-    members->service_name_ + "_Request_";
-  this->setName(name.c_str());
+  std::ostringstream ss;
+  std::string service_namespace(members->service_namespace_);
+  std::string service_name(members->service_name_);
+  if (!service_namespace.empty()) {
+    // Find and replace C namespace separator with C++, in case this is using C typesupport
+    service_namespace = std::regex_replace(service_namespace, std::regex("__"), "::");
+    ss << service_namespace << "::";
+  }
+  ss << "dds_::" << service_name << "_Request_";
+  this->setName(ss.str().c_str());
 
   // Fully bound by default
   this->max_size_bound_ = true;
@@ -60,9 +69,16 @@ ResponseTypeSupport<ServiceMembersType, MessageMembersType>::ResponseTypeSupport
   assert(members);
   this->members_ = members->response_members_;
 
-  std::string name = std::string(members->package_name_) + "::srv::dds_::" +
-    members->service_name_ + "_Response_";
-  this->setName(name.c_str());
+  std::ostringstream ss;
+  std::string service_namespace(members->service_namespace_);
+  std::string service_name(members->service_name_);
+  if (!service_namespace.empty()) {
+    // Find and replace C namespace separator with C++, in case this is using C typesupport
+    service_namespace = std::regex_replace(service_namespace, std::regex("__"), "::");
+    ss << service_namespace << "::";
+  }
+  ss << "dds_::" << service_name << "_Response_";
+  this->setName(ss.str().c_str());
 
   // Fully bound by default
   this->max_size_bound_ = true;

rmw_fastrtps_dynamic_cpp/src/rmw_client.cpp

@@ -108,9 +108,9 @@ rmw_create_client(
   untyped_response_members = get_response_ptr(type_support->data,
       info->typesupport_identifier_);
 
-  std::string request_type_name = _create_type_name(untyped_request_members, "srv",
+  std::string request_type_name = _create_type_name(untyped_request_members,
       info->typesupport_identifier_);
-  std::string response_type_name = _create_type_name(untyped_response_members, "srv",
+  std::string response_type_name = _create_type_name(untyped_response_members,
       info->typesupport_identifier_);
 
   if (!Domain::getRegisteredType(participant, request_type_name.c_str(),

rmw_fastrtps_dynamic_cpp/src/rmw_publisher.cpp

@@ -127,7 +127,7 @@ rmw_create_publisher(
   info->typesupport_identifier_ = type_support->typesupport_identifier;
 
   std::string type_name = _create_type_name(
-    type_support->data, "msg", info->typesupport_identifier_);
+    type_support->data, info->typesupport_identifier_);
   if (!Domain::getRegisteredType(participant, type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->type_support_)))
   {

rmw_fastrtps_dynamic_cpp/src/rmw_service.cpp

@@ -118,9 +118,9 @@ rmw_create_service(
   untyped_response_members = get_response_ptr(type_support->data,
       info->typesupport_identifier_);
 
-  std::string request_type_name = _create_type_name(untyped_request_members, "srv",
+  std::string request_type_name = _create_type_name(untyped_request_members,
       info->typesupport_identifier_);
-  std::string response_type_name = _create_type_name(untyped_response_members, "srv",
+  std::string response_type_name = _create_type_name(untyped_response_members,
       info->typesupport_identifier_);
 
   if (!Domain::getRegisteredType(participant, request_type_name.c_str(),

rmw_fastrtps_dynamic_cpp/src/rmw_subscription.cpp

@@ -130,7 +130,7 @@ rmw_create_subscription(
   info->typesupport_identifier_ = type_support->typesupport_identifier;
 
   std::string type_name = _create_type_name(
-    type_support->data, "msg", info->typesupport_identifier_);
+    type_support->data, info->typesupport_identifier_);
   if (!Domain::getRegisteredType(participant, type_name.c_str(),
     reinterpret_cast<TopicDataType **>(&info->type_support_)))
   {

rmw_fastrtps_dynamic_cpp/src/type_support_common.hpp

@@ -15,6 +15,8 @@
 #ifndef TYPE_SUPPORT_COMMON_HPP_
 #define TYPE_SUPPORT_COMMON_HPP_
 
+#include <regex>
+#include <sstream>
 #include <string>
 
 #include "fastrtps/Domain.h"
@@ -72,31 +74,38 @@ template<typename MembersType>
 ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL
 inline std::string
 _create_type_name(
-  const void * untyped_members,
-  const std::string & sep)
+  const void * untyped_members)
 {
   auto members = static_cast<const MembersType *>(untyped_members);
   if (!members) {
     RMW_SET_ERROR_MSG("members handle is null");
     return "";
   }
-  return
-    std::string(members->package_name_) + "::" + sep + "::dds_::" + members->message_name_ + "_";
+
+  std::ostringstream ss;
+  std::string message_namespace(members->message_namespace_);
+  // Find and replace C namespace separator with C++, in case this is using C typesupport
+  message_namespace = std::regex_replace(message_namespace, std::regex("__"), "::");
+  std::string message_name(members->message_name_);
+  if (!message_namespace.empty()) {
+    ss << message_namespace << "::";
+  }
+  ss << "dds_::" << message_name << "_";
+  return ss.str();
 }
 
 ROSIDL_TYPESUPPORT_INTROSPECTION_CPP_LOCAL
 inline std::string
 _create_type_name(
   const void * untyped_members,
-  const std::string & sep,
   const char * typesupport)
 {
   if (using_introspection_c_typesupport(typesupport)) {
     return _create_type_name<rosidl_typesupport_introspection_c__MessageMembers>(
-      untyped_members, sep);
+      untyped_members);
   } else if (using_introspection_cpp_typesupport(typesupport)) {
     return _create_type_name<rosidl_typesupport_introspection_cpp::MessageMembers>(
-      untyped_members, sep);
+      untyped_members);
   }
   RMW_SET_ERROR_MSG("Unknown typesupport identifier");
   return "";

rmw_fastrtps_shared_cpp/src/demangle.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include <algorithm>
+#include <regex>
 #include <string>
 #include <vector>
 
@@ -32,19 +33,23 @@ _demangle_if_ros_topic(const std::string & topic_name)
 std::string
 _demangle_if_ros_type(const std::string & dds_type_string)
 {
-  std::string substring = "::msg::dds_::";
+  if (dds_type_string[dds_type_string.size() - 1] != '_') {
+    // not a ROS type
+    return dds_type_string;
+  }
+
+  std::string substring = "dds_::";
   size_t substring_position = dds_type_string.find(substring);
-  if (
-    dds_type_string[dds_type_string.size() - 1] == '_' &&
-    substring_position != std::string::npos)
-  {
-    std::string pkg = dds_type_string.substr(0, substring_position);
-    size_t start = substring_position + substring.size();
-    std::string type_name = dds_type_string.substr(start, dds_type_string.length() - 1 - start);
-    return pkg + "/" + type_name;
+  if (substring_position == std::string::npos) {
+    // not a ROS type
+    return dds_type_string;
   }
-  // not a ROS type
-  return dds_type_string;
+
+  std::string type_namespace = dds_type_string.substr(0, substring_position);
+  type_namespace = std::regex_replace(type_namespace, std::regex("::"), "/");
+  size_t start = substring_position + substring.size();
+  std::string type_name = dds_type_string.substr(start, dds_type_string.length() - 1 - start);
+  return type_namespace + type_name;
 }
 
 /// Return the service name for a given topic if it is part of one, else "".
@@ -106,7 +111,7 @@ _demangle_service_from_topic(const std::string & topic_name)
 std::string
 _demangle_service_type_only(const std::string & dds_type_name)
 {
-  std::string ns_substring = "::srv::dds_::";
+  std::string ns_substring = "dds_::";
   size_t ns_substring_position = dds_type_name.find(ns_substring);
   if (std::string::npos == ns_substring_position) {
     // not a ROS service type
@@ -123,7 +128,7 @@ _demangle_service_type_only(const std::string & dds_type_name)
     if (suffix_position != std::string::npos) {
       if (dds_type_name.length() - suffix_position - suffix.length() != 0) {
         RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp",
-          "service type contains '::srv::dds_::' and a suffix, but not at the end"
+          "service type contains 'dds_::' and a suffix, but not at the end"
           ", report this: '%s'", dds_type_name.c_str());
         continue;
       }
@@ -133,13 +138,15 @@ _demangle_service_type_only(const std::string & dds_type_name)
   }
   if (std::string::npos == suffix_position) {
     RCUTILS_LOG_WARN_NAMED("rmw_fastrtps_shared_cpp",
-      "service type contains '::srv::dds_::' but does not have a suffix"
+      "service type contains 'dds_::' but does not have a suffix"
       ", report this: '%s'", dds_type_name.c_str());
     return "";
   }
-  // everything checks out, reformat it from '<pkg>::srv::dds_::<type><suffix>' to '<pkg>/<type>'
-  std::string pkg = dds_type_name.substr(0, ns_substring_position);
+  // everything checks out, reformat it from '[type_namespace::]dds_::<type><suffix>'
+  // to '[type_namespace/]<type>'
+  std::string type_namespace = dds_type_name.substr(0, ns_substring_position);
+  type_namespace = std::regex_replace(type_namespace, std::regex("::"), "/");
   size_t start = ns_substring_position + ns_substring.length();
   std::string type_name = dds_type_name.substr(start, suffix_position - start);
-  return pkg + "/" + type_name;
+  return type_namespace + type_name;
 }