[rmw] Improve handling of dynamic discovery

rmw/CMakeLists.txt

@@ -27,6 +27,7 @@ include(cmake/configure_rmw_library.cmake)
 set(rmw_sources
   "src/allocators.c"
   "src/convert_rcutils_ret_to_rmw_ret.c"
+  "src/discovery_options.c"
   "src/event.c"
   "src/init.c"
   "src/init_options.c"

rmw/include/rmw/discovery_options.h

@@ -0,0 +1,169 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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 RMW__DISCOVERY_OPTIONS_H_
+#define RMW__DISCOVERY_OPTIONS_H_
+
+#include "rcutils/allocator.h"
+
+#include "rmw/macros.h"
+#include "rmw/ret_types.h"
+#include "rmw/visibility_control.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/// Used to control the range that nodes will be discovered
+typedef enum RMW_PUBLIC_TYPE rmw_automatic_discovery_range_e
+{
+  /// Uses ROS_LOCALHOST_ONLY environment variable.
+  RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT = 0,
+  /// Force discovery off
+  RMW_AUTOMATIC_DISCOVERY_RANGE_OFF = 1,
+  /// Allows discovering nodes on the same host
+  RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST = 2,
+  /// Allows discovering nodes on the same subnet
+  RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET = 3,
+} rmw_automatic_discovery_range_t;
+
+/// Maximum length of a peer hostname or IP address
+#define RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH 256
+
+/// Struct to typedef some of the peer addresses
+typedef struct rmw_peer_address_s
+{
+  char peer_address[RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH];
+} rmw_peer_address_t;
+
+/// Used to specify the options that control how discovery is performed
+typedef struct RMW_PUBLIC_TYPE rmw_discovery_options_s
+{
+  /// How far to allow discovering nodes
+  rmw_automatic_discovery_range_t automatic_discovery_range;
+
+  /// The list of manually-specified peers to perform static discovery with
+  /**
+   * Each peer is specified as a hostname or an IP address (IPv4 and IPv6 are both acceptable), or
+   * a subnet, e.g. 192.168.0.0/24.
+   */
+  rmw_peer_address_t * static_peers;
+
+  /// The number of manually-specified peers
+  size_t static_peers_count;
+
+  /// The allocator used to allocate static_peers
+  rcutils_allocator_t allocator;
+} rmw_discovery_options_t;
+
+/// Return a zero-initialized discovery options structure.
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_discovery_options_t
+rmw_get_zero_initialized_discovery_options(void);
+
+/// Initialize a discovery options structure with a set number of static peers.
+/**
+ * This function initializes rmw_discovery_options_t with space for a set number of static peers.
+ *
+ * \param[in] discovery_options Pointer to a zero initialized option structure to be initialized on
+ * success, but left unchanged on failure.
+ * \param[in] size Number of static peers to allocate space for.
+ * \param[in] allocator Allocator to be used to allocate memory.
+ * \returns `RMW_RET_OK` if successful, or
+ * \returns `RMW_RET_INVALID_ARGUMENT` if `discovery_options` is NULL, or
+ * \returns `RMW_RET_INVALID_ARGUMENT` if `discovery_options` is not
+ *   zero initialized, or
+ * \returns `RMW_RET_INVALID_ARGUMENT` if `allocator` is invalid,
+ *   by rcutils_allocator_is_valid() definition, or
+ * \returns `RMW_BAD_ALLOC` if memory allocation fails, or
+ * \returns `RMW_RET_ERROR` when an unspecified error occurs.
+ * \remark This function sets the RMW error state on failure.
+*/
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_ret_t
+rmw_discovery_options_init(
+  rmw_discovery_options_t * discovery_options,
+  size_t size,
+  const rcutils_allocator_t * allocator);
+
+/// Compare two discovery parameter instances for equality.
+/**
+ * Equality means the automatic_discovery_range values are equal, they have the same
+ * static_peers_count value, and each entry in static_peers is evaluated as
+ * equal using strncmp.
+ *
+ * NOTE: If the two parameter structs list the static peers in different orders
+ * then this will evaulate as NOT equal.
+ *
+ * \param[in] left - The first set of options to compare
+ * \param[in] right - The second set of options to compare
+ * \param[out] result - The result of the calculation.
+ *
+ * \return RMW_RET_OK when the input arguments are valid.
+ * \return RMW_RET_INVALID_ARGUMENT will be returned when any input is a nullptr,
+ * or if something in either struct was malformed, such as static_peers being
+ * a nullptr while static_peers_count is non-zero.
+ */
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_ret_t
+rmw_discovery_options_equal(
+  const rmw_discovery_options_t * const left,
+  const rmw_discovery_options_t * const right,
+  bool * result);
+
+/// Perform a deep copy of the discovery options from src into dst using the
+/// given allocator.
+/**
+ * The dst will be left with an owned copy of the static peers array whose
+ * string values match the src.
+ * If successful, src and dst will evaluate as equal using
+ * rmw_discovery_options_equal.
+ *
+ * \param[in] src discovery options to be copied.
+ * \param[in] allocator to use.
+ * \param[out] dst Destination options to use.
+ * \return RMW_RET_OK if success.
+ * \return RMW_RET_INVALID_ARGUMENT if either the src, allocator or dst is null.
+ * \return RMW_RET_BAD_ALLOC if allocation fails.
+ */
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_ret_t
+rmw_discovery_options_copy(
+  const rmw_discovery_options_t * src,
+  const rcutils_allocator_t * allocator,
+  rmw_discovery_options_t * dst);
+
+/// Destructor for rmw_discovery_options_t
+/**
+ * \param[in] discovery_options to destroy
+ * \param[in] allocator to be used for destruction.
+ * \return RMW_RET_OK if success.
+ * \return RMW_RET_INVALID_ARGUMENT if allocator is invalid
+ * or discovery_options is null.
+ */
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_ret_t
+rmw_discovery_options_fini(
+  rmw_discovery_options_t * discovery_options);
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RMW__DISCOVERY_OPTIONS_H_

rmw/include/rmw/impl/cpp/key_value.hpp

@@ -29,7 +29,7 @@ namespace cpp
 // TODO(karsten1987): Implement based on
 // https://github.com/PrismTech/opensplice/blob/master/docs/pdf/OpenSplice_refman_CPP.pdf
 static std::map<std::string, std::vector<uint8_t>>
-parse_key_value(std::vector<uint8_t> kv)
+parse_key_value(const std::vector<uint8_t> & kv)
 {
   std::map<std::string, std::vector<uint8_t>> m;
 

rmw/include/rmw/init_options.h

@@ -23,6 +23,7 @@ extern "C"
 #include <stdint.h>
 
 #include "rcutils/allocator.h"
+#include "rmw/discovery_options.h"
 #include "rmw/domain_id.h"
 #include "rmw/localhost.h"
 #include "rmw/macros.h"
@@ -54,6 +55,8 @@ typedef struct RMW_PUBLIC_TYPE rmw_init_options_s
   rmw_security_options_t security_options;
   /// Enable localhost only
   rmw_localhost_only_t localhost_only;
+  /// Configure discovery
+  rmw_discovery_options_t discovery_options;
   /// Enclave, name used to find security artifacts in a sros2 keystore.
   char * enclave;
 

rmw/src/discovery_options.c

@@ -0,0 +1,165 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+#include <stddef.h>
+#include <string.h>
+
+#include "rmw/discovery_options.h"
+#include "rmw/error_handling.h"
+
+rmw_discovery_options_t
+rmw_get_zero_initialized_discovery_options(void)
+{
+  rmw_discovery_options_t result = (rmw_discovery_options_t) {
+    .automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT,
+    .static_peers_count = 0,
+  };  // NOLINT(readability/braces): false positive
+  return result;
+}
+
+RMW_PUBLIC
+RMW_WARN_UNUSED
+rmw_ret_t
+rmw_discovery_options_init(
+  rmw_discovery_options_t * discovery_options,
+  size_t size,
+  const rcutils_allocator_t * allocator)
+{
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_INVALID_ARGUMENT);
+  RCUTILS_CAN_RETURN_WITH_ERROR_OF(RMW_RET_BAD_ALLOC);
+
+  RMW_CHECK_ARGUMENT_FOR_NULL(discovery_options, RMW_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT);
+
+  if (0 != discovery_options->static_peers_count || NULL != discovery_options->static_peers) {
+    RMW_SET_ERROR_MSG("discovery_options must be zero intialized");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  if (0u == size) {
+    // Nothing to do
+    return RMW_RET_OK;
+  }
+
+  discovery_options->static_peers =
+    allocator->zero_allocate(
+    size,
+    sizeof(rmw_peer_address_t),
+    allocator->state);
+
+  if (NULL == discovery_options->static_peers) {
+    RMW_SET_ERROR_MSG("failed to allocate memory for static_peers");
+    return RMW_RET_BAD_ALLOC;
+  }
+
+  discovery_options->static_peers_count = size;
+  discovery_options->allocator = *allocator;
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t
+rmw_discovery_options_equal(
+  const rmw_discovery_options_t * const left,
+  const rmw_discovery_options_t * const right,
+  bool * result)
+{
+  if (!left || !right || !result) {
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  if (left->automatic_discovery_range != right->automatic_discovery_range) {
+    *result = false;
+    return RMW_RET_OK;
+  }
+
+  if (left->static_peers_count != right->static_peers_count) {
+    *result = false;
+    return RMW_RET_OK;
+  }
+
+  if (left->static_peers_count == 0) {
+    /* No need to examine the static_peers arrays if the count is zero */
+    *result = true;
+    return RMW_RET_OK;
+  }
+
+  if (!left->static_peers || !right->static_peers) {
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  for (size_t ii = 0; ii < left->static_peers_count; ++ii) {
+    if (strncmp(
+        left->static_peers[ii].peer_address,
+        right->static_peers[ii].peer_address,
+        RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH) != 0)
+    {
+      *result = false;
+      return RMW_RET_OK;
+    }
+  }
+
+  *result = true;
+  return RMW_RET_OK;
+}
+
+rmw_ret_t
+rmw_discovery_options_copy(
+  const rmw_discovery_options_t * src,
+  const rcutils_allocator_t * allocator,
+  rmw_discovery_options_t * dst)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(src, RMW_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ALLOCATOR(allocator, return RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(dst, RMW_RET_INVALID_ARGUMENT);
+  if (src == dst) {
+    RMW_SET_ERROR_MSG("src and dst must be different");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  rmw_ret_t ret = rmw_discovery_options_init(dst, src->static_peers_count, allocator);
+  if (RMW_RET_OK != ret) {
+    return ret;
+  }
+
+  dst->automatic_discovery_range = src->automatic_discovery_range;
+
+  for (size_t i = 0; i < src->static_peers_count; i++) {
+    strncpy(
+      dst->static_peers[i].peer_address,
+      src->static_peers[i].peer_address,
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH);
+    dst->static_peers[i].peer_address[
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH - 1] = '\0';
+  }
+
+  return RMW_RET_OK;
+}
+
+rmw_ret_t
+rmw_discovery_options_fini(
+  rmw_discovery_options_t * discovery_options)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(discovery_options, RMW_RET_INVALID_ARGUMENT);
+
+  if (discovery_options->static_peers_count > 0) {
+    RCUTILS_CHECK_ALLOCATOR(&(discovery_options->allocator), return RMW_RET_INVALID_ARGUMENT);
+    discovery_options->allocator.deallocate(
+      discovery_options->static_peers,
+      discovery_options->allocator.state);
+  }
+  *discovery_options = rmw_get_zero_initialized_discovery_options();
+
+  return RMW_RET_OK;
+}

rmw/src/init_options.c

@@ -28,6 +28,10 @@ rmw_get_zero_initialized_init_options(void)
   return (const rmw_init_options_t) {
            .domain_id = RMW_DEFAULT_DOMAIN_ID,
            .localhost_only = RMW_LOCALHOST_ONLY_DEFAULT,
+           .discovery_options = {
+             .automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT,
+             .static_peers_count = 0
+           },
            .implementation_identifier = NULL,
            .impl = NULL,
            .instance_id = 0,

rmw/test/CMakeLists.txt

@@ -20,6 +20,15 @@ if(TARGET test_convert_rcutils_ret_to_rmw_ret)
   target_link_libraries(test_convert_rcutils_ret_to_rmw_ret ${PROJECT_NAME})
 endif()
 
+ament_add_gmock(test_discovery_options
+  test_discovery_options.cpp
+  # Append the directory of librmw so it is found at test time.
+  APPEND_LIBRARY_DIRS "$<TARGET_FILE_DIR:${PROJECT_NAME}>"
+)
+if(TARGET test_discovery_options)
+  target_link_libraries(test_discovery_options ${PROJECT_NAME})
+endif()
+
 ament_add_gmock(test_event
   test_event.cpp
   # Append the directory of librmw so it is found at test time.