Node graph impl

rmw_opensplice_cpp/CMakeLists.txt

@@ -58,6 +58,7 @@ add_library(rmw_opensplice_cpp SHARED
   src/rmw_guard_condition.cpp
   src/rmw_init.cpp
   src/rmw_node.cpp
+  src/rmw_node_info_and_types.cpp
   src/rmw_node_names.cpp
   src/rmw_publish.cpp
   src/rmw_publisher.cpp
@@ -83,7 +84,8 @@ ament_target_dependencies(rmw_opensplice_cpp
   "rosidl_generator_cpp"
   "rosidl_typesupport_opensplice_c"
   "rosidl_typesupport_opensplice_cpp"
-  "OpenSplice")
+  "OpenSplice"
+)
 # On Windows this adds the RMW_BUILDING_DLL definition.
 # On Unix (GCC or Clang) it hides the symbols with -fvisibility=hidden.
 configure_rmw_library(rmw_opensplice_cpp)

rmw_opensplice_cpp/src/guid.hpp

@@ -0,0 +1,151 @@
+// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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.
+
+#ifndef GUID_HPP_
+#define GUID_HPP_
+
+#include <dds_dcps.h>
+
+#include <cstring>
+#include <iostream>
+#include <map>
+#include <set>
+#include <string>
+#include <vector>
+
+// TODO(ross-desmond): This should all be in opensplice
+typedef char octet;
+
+/**
+ * Structure to hold GUID information for DDS instances.
+ * http://www.eprosima.com/docs/fast-rtps/1.6.0/html/_guid_8h_source.html
+ *
+ */
+struct GuidPrefix_t
+{
+  static constexpr unsigned int kSize = 12;
+  octet value[kSize];
+
+  GuidPrefix_t()
+  {
+    memset(value, 0, kSize);
+  }
+
+  explicit GuidPrefix_t(octet guid[kSize])
+  {
+    memcpy(value, guid, kSize);
+  }
+
+  GuidPrefix_t(const GuidPrefix_t & g)
+  {
+    memcpy(value, g.value, kSize);
+  }
+
+  GuidPrefix_t(GuidPrefix_t && g)
+  {
+    memmove(value, g.value, kSize);
+  }
+
+  GuidPrefix_t & operator=(const GuidPrefix_t & guidpre)
+  {
+    memcpy(value, guidpre.value, kSize);
+    return *this;
+  }
+
+  GuidPrefix_t & operator=(GuidPrefix_t && guidpre)
+  {
+    memmove(value, guidpre.value, kSize);
+    return *this;
+  }
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS_PUBLIC
+
+  bool operator==(const GuidPrefix_t & prefix) const
+  {
+    return memcmp(value, prefix.value, kSize) == 0;
+  }
+
+  bool operator!=(const GuidPrefix_t & prefix) const
+  {
+    return memcmp(value, prefix.value, kSize) != 0;
+  }
+
+#endif
+};
+
+inline bool operator<(const GuidPrefix_t & g1, const GuidPrefix_t & g2)
+{
+  for (uint8_t i = 0; i < GuidPrefix_t::kSize; ++i) {
+    if (g1.value[i] < g2.value[i]) {
+      return true;
+    } else if (g1.value[i] > g2.value[i]) {
+      return false;
+    }
+  }
+  return false;
+}
+
+inline std::ostream & operator<<(std::ostream & output, const GuidPrefix_t & guiP)
+{
+  output << std::hex;
+  for (uint8_t i = 0; i < GuidPrefix_t::kSize - 1; ++i) {
+    output << static_cast<int>(guiP.value[i]) << ".";
+  }
+  output << static_cast<int>(guiP.value[GuidPrefix_t::kSize - 1]);
+  return output << std::dec;
+}
+
+// TODO(ross-desmond): check this with opensplice source code to ensure compatibility
+/**
+ * Convert an instance handle to a guid.
+ *
+ * @param guid [out] the resulting guid
+ * @param domain_id to prepend to the guid
+ * @param instance_handle to append to the guid
+ */
+inline void DDS_InstanceHandle_to_GUID(
+  struct GuidPrefix_t * guid,
+  DDS::DomainId_t domain_id,
+  DDS::InstanceHandle_t instance_handle)
+{
+  memcpy(guid->value, reinterpret_cast<char *>(&domain_id), sizeof(DDS::DomainId_t));
+  memcpy(guid->value + sizeof(DDS::DomainId_t),
+    reinterpret_cast<char *>(&instance_handle), sizeof(DDS::InstanceHandle_t));
+}
+
+inline void DDS_BuiltinTopicKey_to_GUID(
+  struct GuidPrefix_t * guid,
+  DDS::BuiltinTopicKey_t buitinTopicKey)
+{
+#if BIG_ENDIAN
+  /* Big Endian */
+  memcpy(guid->value, reinterpret_cast<octet *>(buitinTopicKey), GuidPrefix_t::kSize);
+#else
+  /* Little Endian */
+  int i;
+  octet * guidElement;
+  octet * topicKeyBuffer = reinterpret_cast<octet *>(buitinTopicKey);
+  octet * keyBufferElement;
+  for (i = 0; i < 3; ++i) {
+    octet * guidElement = &guid->value[i * 3];
+    octet * keyBufferElement = reinterpret_cast<octet *>(&buitinTopicKey[i * 3]);
+    guidElement[0] = keyBufferElement[2];
+    guidElement[1] = keyBufferElement[1];
+    guidElement[2] = keyBufferElement[0];
+  }
+
+#endif
+}
+
+#endif  // GUID_HPP_

rmw_opensplice_cpp/src/names_and_types_helpers.hpp

@@ -0,0 +1,222 @@
+// Copyright 2018 Amazon.com, Inc. or its affiliates. All Rights Reserved.
+//
+// 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.
+
+#ifndef NAMES_AND_TYPES_HELPERS_HPP_
+#define NAMES_AND_TYPES_HELPERS_HPP_
+
+#define SAMPLE_PREFIX "/Sample_"
+
+#include <map>
+#include <set>
+#include <string>
+
+#include "identifier.hpp"
+#include "types.hpp"
+#include "demangle.hpp"
+
+#include "rcutils/allocator.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/strdup.h"
+#include "rmw/error_handling.h"
+#include "rmw/names_and_types.h"
+#include "rmw/get_service_names_and_types.h"
+#include "rmw/types.h"
+#include "rmw/convert_rcutils_ret_to_rmw_ret.h"
+#include "rmw/impl/cpp/macros.hpp"
+
+
+// The extern "C" here enforces that overloading is not used.
+extern "C"
+{
+inline rmw_ret_t
+validate_node(
+  const rmw_node_t * node,
+  rcutils_allocator_t * allocator)
+{
+  if (!allocator) {
+    RMW_SET_ERROR_MSG("allocator is null");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+  if (!node) {
+    RMW_SET_ERROR_MSG("null node handle");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    node
+    handle,
+    node->implementation_identifier, opensplice_cpp_identifier,
+    return RMW_RET_ERROR)
+
+  auto node_info = static_cast<OpenSpliceStaticNodeInfo *>(node->data);
+  if (!node_info) {
+    RMW_SET_ERROR_MSG("node info handle is null");
+  }
+  if (!node_info->publisher_listener) {
+    RMW_SET_ERROR_MSG("publisher listener handle is null");
+    return RMW_RET_ERROR;
+  }
+  if (!node_info->publisher_listener) {
+    RMW_SET_ERROR_MSG("publisher listener handle is null");
+    return RMW_RET_ERROR;
+  }
+  if (!node_info->subscriber_listener) {
+    RMW_SET_ERROR_MSG("subscriber listener handle is null");
+    return RMW_RET_ERROR;
+  }
+  return RMW_RET_OK;
+}
+
+inline rmw_ret_t
+copy_topics_names_and_types(
+  const std::map<std::string, std::set<std::string>> & topics,
+  rcutils_allocator_t * allocator,
+  bool no_demangle,
+  rmw_names_and_types_t * topic_names_and_types)
+{
+  // Copy data to results handle
+  if (topics.size() > 0) {
+    // Setup string array to store names
+    rmw_ret_t rmw_ret = rmw_names_and_types_init(topic_names_and_types, topics.size(), allocator);
+    if (rmw_ret != RMW_RET_OK) {
+      return rmw_ret;
+    }
+    // Setup cleanup function, in case of failure below
+    auto fail_cleanup = [&topic_names_and_types]() {
+        rmw_ret_t rmw_ret = rmw_names_and_types_fini(topic_names_and_types);
+        if (rmw_ret != RMW_RET_OK) {
+          RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str);
+        }
+      };
+    // Setup demangling functions based on no_demangle option
+    auto demangle_topic = _demangle_if_ros_topic;
+    auto demangle_type = _demangle_if_ros_type;
+    if (no_demangle) {
+      auto noop = [](const std::string & in) {
+          return in;
+        };
+      demangle_topic = noop;
+      demangle_type = noop;
+    }
+    // For each topic, store the name, initialize the string array for types, and store all types
+    size_t index = 0;
+    for (const auto & topic_n_types : topics) {
+      // Duplicate and store the topic_name
+      char * topic_name = rcutils_strdup(demangle_topic(topic_n_types.first).c_str(), *allocator);
+      if (!topic_name) {
+        RMW_SET_ERROR_MSG("failed to allocate memory for topic name");
+        fail_cleanup();
+        return RMW_RET_BAD_ALLOC;
+      }
+      topic_names_and_types->names.data[index] = topic_name;
+      // Setup storage for types
+      {
+        rcutils_ret_t rcutils_ret = rcutils_string_array_init(
+          &topic_names_and_types->types[index],
+          topic_n_types.second.size(),
+          allocator);
+        if (rcutils_ret != RCUTILS_RET_OK) {
+          RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
+          fail_cleanup();
+          return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret);
+        }
+      }
+      // Duplicate and store each type for the topic
+      size_t type_index = 0;
+      for (const auto & type : topic_n_types.second) {
+        char * type_name = rcutils_strdup(demangle_type(type).c_str(), *allocator);
+        if (!type_name) {
+          RMW_SET_ERROR_MSG("failed to allocate memory for type name");
+          fail_cleanup();
+          return RMW_RET_BAD_ALLOC;
+        }
+        topic_names_and_types->types[index].data[type_index] = type_name;
+        ++type_index;
+      }  // for each type
+      ++index;
+    }  // for each topic
+  }
+  return RMW_RET_OK;
+}
+
+inline rmw_ret_t
+copy_services_to_names_and_types(
+  const std::map<std::string, std::set<std::string>> & services,
+  rcutils_allocator_t * allocator,
+  rmw_names_and_types_t * service_names_and_types)
+{
+  // Fill out service_names_and_types
+  if (services.size()) {
+    // Setup string array to store names
+    rmw_ret_t rmw_ret =
+      rmw_names_and_types_init(service_names_and_types, services.size(), allocator);
+    if (rmw_ret != RMW_RET_OK) {
+      return rmw_ret;
+    }
+    // Setup cleanup function, in case of failure below
+    auto fail_cleanup = [&service_names_and_types]() {
+        rmw_ret_t rmw_ret = rmw_names_and_types_fini(service_names_and_types);
+        if (rmw_ret != RMW_RET_OK) {
+          RCUTILS_LOG_ERROR("error during report of error: %s", rmw_get_error_string().str);
+        }
+      };
+    // For each service, store the name, initialize the string array for types, and store all types
+    size_t index = 0;
+    for (const auto & service_n_types : services) {
+      // Duplicate and store the service_name
+      char * service_name = rcutils_strdup(service_n_types.first.c_str(), *allocator);
+      if (!service_name) {
+        RMW_SET_ERROR_MSG("failed to allocate memory for service name");
+        fail_cleanup();
+        return RMW_RET_BAD_ALLOC;
+      }
+      service_names_and_types->names.data[index] = service_name;
+      // Setup storage for types
+      {
+        rcutils_ret_t rcutils_ret = rcutils_string_array_init(
+          &service_names_and_types->types[index],
+          service_n_types.second.size(),
+          allocator);
+        if (rcutils_ret != RCUTILS_RET_OK) {
+          RMW_SET_ERROR_MSG(rcutils_get_error_string().str);
+          fail_cleanup();
+          return rmw_convert_rcutils_ret_to_rmw_ret(rcutils_ret);
+        }
+      }
+      // Duplicate and store each type for the service
+      size_t type_index = 0;
+      for (const auto & type : service_n_types.second) {
+        // Strip the SAMPLE_PREFIX if it is found (e.g. from services using OpenSplice typesupport).
+        // It may not be found if services are detected using other typesupports.
+        size_t n = type.find(SAMPLE_PREFIX);
+        std::string stripped_type = type;
+        if (std::string::npos != n) {
+          stripped_type = type.substr(0, n + 1) + type.substr(n + strlen(SAMPLE_PREFIX));
+        }
+        char * type_name = rcutils_strdup(stripped_type.c_str(), *allocator);
+        if (!type_name) {
+          RMW_SET_ERROR_MSG("failed to allocate memory for type name");
+          fail_cleanup();
+          return RMW_RET_BAD_ALLOC;
+        }
+        service_names_and_types->types[index].data[type_index] = type_name;
+        ++type_index;
+      }  // for each type
+      ++index;
+    }  // for each service
+  }
+  return RMW_RET_OK;
+}
+}  // extern "C"
+
+#endif  // NAMES_AND_TYPES_HELPERS_HPP_