[rcl] Improve handling of dynamic discovery

rcl/CMakeLists.txt

@@ -38,6 +38,7 @@ set(${PROJECT_NAME}_sources
   src/rcl/client.c
   src/rcl/common.c
   src/rcl/context.c
+  src/rcl/discovery_options.c
   src/rcl/domain_id.c
   src/rcl/event.c
   src/rcl/expand_topic_name.c

rcl/include/rcl/discovery_options.h

@@ -0,0 +1,88 @@
+// 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.
+
+/// @file
+
+#ifndef RCL__DISCOVERY_OPTIONS_H_
+#define RCL__DISCOVERY_OPTIONS_H_
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include "rcl/types.h"
+#include "rcl/visibility_control.h"
+
+#include "rcutils/allocator.h"
+
+#include "rmw/discovery_options.h"
+
+/// Determine how the user wishes to discover other ROS nodes automatically.
+/**
+ * Checks an environment variable to determine how far automatic discovery should be allowed to
+ * propagate: not at all, the local machine only, or however far the automatic discovery mechanism
+ * used by the RMW implementation can propagate on the network (e.g. for multicast-based discovery,
+ * this will be the local subnet).
+ *
+ * \param[out] discovery_options Must not be NULL.
+ * \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or
+ * \return #RCL_RET_ERROR if an unexpected error happened, or
+ * \return #RCL_RET_OK.
+ */
+RCL_PUBLIC
+rcl_ret_t
+rcl_get_discovery_automatic_range(rmw_discovery_options_t * discovery_options);
+
+/// Convert the automatic discovery range value to a string for easy printing.
+/**
+ * The string buffer passed to this function should be at least 40 bytes. If it is less (as
+ * indicated by the size parameter) the stringified enumeration value will be truncated.
+ *
+ * \param[in] destination The string buffer to print into. Must not be NULL.
+ * \param[in] size Must be the size of the destination buffer, including space for a NULL byte.
+ * \param[in] discovery_options Must not be NULL.
+ * \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or
+ * \return #RCL_RET_ERROR if an unexpected error happened, or
+ * \return #RCL_RET_OK.
+ */
+RCL_PUBLIC
+rcl_ret_t
+rcl_automatic_discovery_range_to_string(
+  char * destination,
+  size_t size,
+  rmw_discovery_options_t * discovery_options);
+
+/// Determine how the user wishes to discover other ROS nodes via statically-configured peers.
+/**
+ * Checks an environment variable to determine the hosts that the user wants to communicate with,
+ * in addition to localhost.
+ * TODO(gbiggs): Where should the IPs be validated?
+ *
+ * \param[out] discovery_options Must not be NULL.
+ * \return #RCL_RET_INVALID_ARGUMENT if an argument is invalid, or
+ * \return #RCL_RET_ERROR if an unexpected error happened, or
+ * \return #RCL_RET_OK.
+ */
+RCL_PUBLIC
+rcl_ret_t
+rcl_get_discovery_static_peers(
+  rmw_discovery_options_t * discovery_options,
+  rcutils_allocator_t * allocator);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // RCL__DISCOVERY_OPTIONS_H_

rcl/src/rcl/discovery_options.c

@@ -0,0 +1,178 @@
+// 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 "rcl/discovery_options.h"
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "rcutils/allocator.h"
+#include "rcutils/env.h"
+#include "rcutils/logging_macros.h"
+#include "rcutils/split.h"
+#include "rcutils/types/string_array.h"
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+
+static const char * const RCL_STATIC_PEERS_ENV_VAR = "ROS_STATIC_PEERS";
+static const char * const RCL_AUTOMATIC_DISCOVERY_RANGE_ENV_VAR = "ROS_AUTOMATIC_DISCOVERY_RANGE";
+
+rcl_ret_t
+rcl_get_discovery_automatic_range(rmw_discovery_options_t * discovery_options)
+{
+  const char * ros_automatic_discovery_range_env_val = NULL;
+  const char * get_env_error_str = NULL;
+
+  RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+  RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR);
+  RCL_CHECK_ARGUMENT_FOR_NULL(discovery_options, RCL_RET_INVALID_ARGUMENT);
+
+  get_env_error_str = rcutils_get_env(
+    RCL_AUTOMATIC_DISCOVERY_RANGE_ENV_VAR,
+    &ros_automatic_discovery_range_env_val);
+  if (NULL != get_env_error_str) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Error getting env var '%s': %s", RCL_AUTOMATIC_DISCOVERY_RANGE_ENV_VAR,
+      get_env_error_str);
+    return RCL_RET_ERROR;
+  }
+  if (ros_automatic_discovery_range_env_val == NULL) {
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST;
+  } else if (strcmp(ros_automatic_discovery_range_env_val, "OFF") == 0) {
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_OFF;
+  } else if (strcmp(ros_automatic_discovery_range_env_val, "LOCALHOST") == 0) {
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST;
+  } else if (strcmp(ros_automatic_discovery_range_env_val, "SUBNET") == 0) {
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET;
+  } else {
+    RCUTILS_LOG_WARN_NAMED(
+      ROS_PACKAGE_NAME,
+      "Invalid value %s specified for '%s''; assuming localhost only",
+      ros_automatic_discovery_range_env_val,
+      RCL_AUTOMATIC_DISCOVERY_RANGE_ENV_VAR);
+
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST;
+  }
+
+  return RCL_RET_OK;
+}
+
+RCL_PUBLIC
+rcl_ret_t
+rcl_automatic_discovery_range_to_string(
+  char * destination,
+  size_t size,
+  rmw_discovery_options_t * discovery_options)
+{
+  RCL_CHECK_ARGUMENT_FOR_NULL(discovery_options, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(destination, RCL_RET_INVALID_ARGUMENT);
+
+  switch (discovery_options->automatic_discovery_range) {
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_OFF:
+      snprintf(
+        destination,
+        size,
+        "RMW_AUTOMATIC_DISCOVERY_RANGE_OFF (%d)",
+        discovery_options->automatic_discovery_range);
+      break;
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST:
+      snprintf(
+        destination,
+        size,
+        "RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST (%d)",
+        discovery_options->automatic_discovery_range);
+      break;
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET:
+      snprintf(
+        destination,
+        size,
+        "RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET (%d)",
+        discovery_options->automatic_discovery_range);
+      break;
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT:
+    default:
+      snprintf(
+        destination,
+        size,
+        "RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT (%d)",
+        discovery_options->automatic_discovery_range);
+      break;
+  }
+
+  return RCL_RET_OK;
+}
+
+rcl_ret_t
+rcl_get_discovery_static_peers(
+  rmw_discovery_options_t * discovery_options,
+  rcutils_allocator_t * allocator)
+{
+  const char * ros_peers_env_val = NULL;
+  const char * get_env_error_str = NULL;
+
+  RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_INVALID_ARGUMENT);
+  RCUTILS_CAN_SET_MSG_AND_RETURN_WITH_ERROR_OF(RCL_RET_ERROR);
+  RCL_CHECK_ARGUMENT_FOR_NULL(discovery_options, RCL_RET_INVALID_ARGUMENT);
+  RCL_CHECK_ARGUMENT_FOR_NULL(allocator, RCL_RET_INVALID_ARGUMENT);
+
+  get_env_error_str = rcutils_get_env(RCL_STATIC_PEERS_ENV_VAR, &ros_peers_env_val);
+  if (NULL != get_env_error_str) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Error getting env var '%s': %s",
+      RCL_STATIC_PEERS_ENV_VAR, get_env_error_str);
+    return RCL_RET_ERROR;
+  }
+
+  discovery_options->static_peers_count = 0;
+  if (ros_peers_env_val != NULL) {
+    rcutils_string_array_t array = rcutils_get_zero_initialized_string_array();
+    rcutils_ret_t split_ret = rcutils_split(ros_peers_env_val, ';', *allocator, &array);
+    if (RCUTILS_RET_OK != split_ret) {
+      RCL_SET_ERROR_MSG(rcutils_get_error_string().str);
+      return RCL_RET_ERROR;
+    }
+
+    discovery_options->static_peers =
+      allocator->zero_allocate(
+      array.size,
+      sizeof(rmw_peer_address_t),
+      allocator->state);
+
+    for (size_t i = 0; i < array.size; ++i) {
+      if (strlen(array.data[i]) > (RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH - 1)) {
+        RCUTILS_LOG_WARN_NAMED(
+          ROS_PACKAGE_NAME,
+          "Static peer %s specified to '%s' is too long (maximum of %d); skipping",
+          array.data[i], RCL_STATIC_PEERS_ENV_VAR,
+          RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH - 1);
+        continue;
+      }
+      strncpy(
+        discovery_options->static_peers[discovery_options->static_peers_count].peer_address,
+        array.data[i],
+        RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH);
+      discovery_options->static_peers[discovery_options->static_peers_count].peer_address[
+        RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH - 1] = '\0';
+      discovery_options->static_peers_count++;
+    }
+
+    if (RCUTILS_RET_OK != rcutils_string_array_fini(&array)) {
+      RCL_SET_ERROR_MSG(rcutils_get_error_string().str);
+      // We don't fail here because we got the work done, we will just leak memory
+    }
+  }
+
+  return RCL_RET_OK;
+}

rcl/src/rcl/init.c

@@ -28,6 +28,7 @@ extern "C"
 #include "tracetools/tracetools.h"
 
 #include "rcl/arguments.h"
+#include "rcl/discovery_options.h"
 #include "rcl/domain_id.h"
 #include "rcl/error_handling.h"
 #include "rcl/localhost.h"
@@ -155,13 +156,74 @@ rcl_init(
 
   rmw_localhost_only_t * localhost_only =
     &context->impl->init_options.impl->rmw_init_options.localhost_only;
-  if (RMW_LOCALHOST_ONLY_DEFAULT == *localhost_only) {
+  if (RMW_LOCALHOST_ONLY_DEFAULT != *localhost_only) {
+    RCUTILS_LOG_WARN_NAMED(
+      ROS_PACKAGE_NAME,
+      "'localhost_only' init option is deprecated. Use 'automatic_discovery_range' and "
+      "'static_peers' instead.");
+  } else {
     // Get actual localhost_only value based on environment variable, if needed.
     ret = rcl_get_localhost_only(localhost_only);
     if (RCL_RET_OK != ret) {
       fail_ret = ret;
       goto fail;
     }
+    if (RMW_LOCALHOST_ONLY_DEFAULT != *localhost_only) {
+      RCUTILS_LOG_WARN_NAMED(
+        ROS_PACKAGE_NAME,
+        "ROS_LOCALHOST_ONLY is deprecated. Use ROS_AUTOMATIC_DISCOVERY_RANGE_DEFAULT and "
+        "ROS_STATIC_PEERS instead.");
+    }
+  }
+
+  rmw_discovery_options_t * discovery_options =
+    &context->impl->init_options.impl->rmw_init_options.discovery_options;
+  if (RMW_AUTOMATIC_DISCOVERY_RANGE_DEFAULT == discovery_options->automatic_discovery_range) {
+    // Get actual multicast discovery value based on environment variable, if needed
+    ret = rcl_get_discovery_automatic_range(discovery_options);
+    if (RCL_RET_OK != ret) {
+      fail_ret = ret;
+      goto fail;
+    }
+  }
+
+  if (0 == discovery_options->static_peers_count &&
+    discovery_options->automatic_discovery_range != RMW_AUTOMATIC_DISCOVERY_RANGE_OFF)
+  {
+    // Get static peers.
+    // If off is set, it makes sense to not get any static peers.
+    ret = rcl_get_discovery_static_peers(discovery_options, &allocator);
+    if (RCL_RET_OK != ret) {
+      fail_ret = ret;
+      goto fail;
+    }
+  }
+
+  if (discovery_options->static_peers_count > 0 &&
+    discovery_options->automatic_discovery_range == RMW_AUTOMATIC_DISCOVERY_RANGE_OFF)
+  {
+    RCUTILS_LOG_WARN_NAMED(
+      ROS_PACKAGE_NAME,
+      "Note: ROS_AUTOMATIC_DISCOVERY_RANGE is set to OFF, but "
+      "found static peers in ROS_STATIC_PEERS. "
+      "Your settings ROS_STATIC_PEERS will be ignored.");
+  }
+
+  char discovery_range_string[41];  // TODO(gbiggs) Is constexpr available in C11?
+  rcl_automatic_discovery_range_to_string(discovery_range_string, 41, discovery_options);
+  RCUTILS_LOG_INFO_NAMED(
+    ROS_PACKAGE_NAME,
+    "Automatic discovery range is %s",
+    discovery_range_string);
+  RCUTILS_LOG_INFO_NAMED(
+    ROS_PACKAGE_NAME,
+    "Static peers count is %lu",
+    discovery_options->static_peers_count);
+
+  for (size_t ii = 0; ii < discovery_options->static_peers_count; ++ii) {
+    RCUTILS_LOG_INFO_NAMED(
+      ROS_PACKAGE_NAME,
+      "\t%s", discovery_options->static_peers[ii].peer_address);
   }
 
   if (context->global_arguments.impl->enclave) {

rcl/src/rcl/localhost.c

@@ -41,9 +41,11 @@ rcl_get_localhost_only(rmw_localhost_only_t * localhost_only)
       get_env_error_str);
     return RCL_RET_ERROR;
   }
-  *localhost_only = (ros_local_host_env_val != NULL &&
-    strcmp(
-      ros_local_host_env_val,
-      "1") == 0) ? RMW_LOCALHOST_ONLY_ENABLED : RMW_LOCALHOST_ONLY_DISABLED;
+  if (ros_local_host_env_val == NULL || ros_local_host_env_val[0] == '\0') {
+    *localhost_only = RMW_LOCALHOST_ONLY_DEFAULT;
+  } else {
+    *localhost_only = strcmp(
+      ros_local_host_env_val, "1") == 0 ? RMW_LOCALHOST_ONLY_ENABLED : RMW_LOCALHOST_ONLY_DISABLED;
+  }
   return RCL_RET_OK;
 }

rcl/test/CMakeLists.txt

@@ -398,6 +398,12 @@ rcl_add_custom_gtest(test_validate_enclave_name
   LIBRARIES ${PROJECT_NAME} mimick
 )
 
+rcl_add_custom_gtest(test_discovery_options
+  SRCS rcl/test_discovery_options.cpp
+  APPEND_LIBRARY_DIRS ${extra_lib_dirs}
+  LIBRARIES ${PROJECT_NAME}
+)
+
 rcl_add_custom_gtest(test_domain_id
   SRCS rcl/test_domain_id.cpp
   APPEND_LIBRARY_DIRS ${extra_lib_dirs}