[rcl] Improve handling of dynamic discovery (ros2#1023)

* Get discovery preferences from the environment

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>

* Support specification of discovery range and static peers

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>

* Add some debug helpers

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>

* Cleanup the LOCALHOST_ONLY deprecation a bit.

Only print out the warning if it is actually specified.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Rewrite parsing of static peers.

Mostly to get rid of the use of strtok_r, which is dangerous
and also should not be used on our static environment variables.
Instead, use rcutils_split(), which is much better.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Fix some silly bugs.

Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>

* Use names instead of integers for discovery range env vars

Signed-off-by: Michael X. Grey <grey@openrobotics.org>

* Add support for dynamic allocation

This commit adds support for dynamic allocation for unlimited number of
static peers.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Add a warning if ROS_AUTOMATIC_DISCOVERY_OFF is set and STATIC_PEERS are
set.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Update to latest rmw API

Signed-off-by: Michael X. Grey <grey@openrobotics.org>

* Uncrustify

Signed-off-by: Michael X. Grey <grey@openrobotics.org>

* Update for rmw_discovery_options_t changes

Signed-off-by: Shane Loretz <sloretz@google.com>

* Address feedback: use strncmp

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address feedback: Log levels

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address feedback: remove TODO

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address feedback: rename function

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* ddress feedback: Docstring

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Address feedback: comment

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Add `RCL_RET_ERROR` if fini fails.

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* `snprintf`->`rcutils_snprintf`

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Rename tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* rcl_get_discovery_automatic_range -> rcl_get_automatic_discovery_range in test

Signed-off-by: Shane Loretz <sloretz@google.com>

* Annotate tests

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* More comments

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Style

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Uncrustify

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* Constness and warning

Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>

* address TODO about IP address validation in the static peers

Signed-off-by: William Woodall <william@osrfoundation.org>

* NOT_SET and SYSTEM_DEFAULT values

Signed-off-by: Shane Loretz <sloretz@google.com>

* refactor discovery options to handle env vars better and simplify stringifying enums

Signed-off-by: William Woodall <william@osrfoundation.org>

* fixup docs

Signed-off-by: William Woodall <william@osrfoundation.org>

* (re)improve the discovery range debug message

Signed-off-by: William Woodall <william@osrfoundation.org>

* Set discovery options to NOT_SET to detect user changse

Signed-off-by: Shane Loretz <sloretz@google.com>

* lint

Signed-off-by: Shane Loretz <sloretz@google.com>

* strncpy_s on windows

Signed-off-by: Shane Loretz <sloretz@google.com>

* Change default range to SUBNET, and allow configuring it at build time

Signed-off-by: Shane Loretz <sloretz@google.com>

---------

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>
Signed-off-by: Chris Lalancette <clalancette@openrobotics.org>
Signed-off-by: Michael X. Grey <grey@openrobotics.org>
Signed-off-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Signed-off-by: Shane Loretz <sloretz@google.com>
Signed-off-by: William Woodall <william@osrfoundation.org>
Co-authored-by: Chris Lalancette <clalancette@openrobotics.org>
Co-authored-by: Michael X. Grey <grey@openrobotics.org>
Co-authored-by: Arjo Chakravarty <arjoc@intrinsic.ai>
Co-authored-by: Shane Loretz <sloretz@google.com>
Co-authored-by: William Woodall <william@osrfoundation.org>

rcl/CMakeLists.txt

@@ -41,6 +41,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
@@ -98,6 +99,12 @@ target_link_libraries(${PROJECT_NAME} PRIVATE
   yaml
 )
 
+# Allow configuring the default discovery range
+if(DEFINED RCL_DEFAULT_DISCOVERY_RANGE)
+target_compile_definitions(${PROJECT_NAME} PRIVATE
+  "RCL_DEFAULT_DISCOVERY_RANGE=${RCL_DEFAULT_DISCOVERY_RANGE}")
+endif()
+
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(${PROJECT_NAME} PRIVATE "RCL_BUILDING_DLL")

rcl/include/rcl/discovery_options.h

@@ -0,0 +1,109 @@
+// 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.
+/**
+ * Use the ROS_AUTOMATIC_DISCOVERY_RANGE environment variable to determine how
+ * far automatic discovery should be allowed to propagate:
+ *
+ *   - not at all (no automatic discovery),
+ *   - the local machine only,
+ *   - or the subnet (or as far beyond the local system as the middleware can)
+ *
+ * When the subnet is specified the automatic discovery mechanism used by the
+ * rmw implementation dictates how far discovery can propagate on the network,
+ * e.g. for multicast-based discovery this will be the local subnet, hence the
+ * name.
+ *
+ * The option indicated by the environment variable will be stored in the
+ * automatic_discovery_range field of the given discovery_options struct.
+ *
+ * If the environment variable isn't set, then the default discovery range
+ * will be the value of the preprocessor definition
+ * `RCL_DEFAULT_DISCOVERY_RANGE`.
+ * If the definition is undefined, then the default will be SUBNET.
+ * It is intended that the default will be LOCALHOST in future versions of ROS.
+ *
+ * \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_automatic_discovery_range(rmw_discovery_options_t * discovery_options);
+
+/// Convert the automatic discovery range value to a string for easy printing.
+/**
+ * \param[in] automatic_discovery_range range enum to stringify
+ * \return string version of enum, or NULL if not recognized
+ */
+RCL_PUBLIC
+const char *
+rcl_automatic_discovery_range_to_string(rmw_automatic_discovery_range_t automatic_discovery_range);
+
+/// Determine how the user wishes to discover other ROS nodes via statically-configured peers.
+/**
+ * Use the ROS_STATIC_PEERS environment variable to determine which hosts
+ * the user wants to communicate with, in addition to localhost.
+ *
+ * Values for the static peers are not validated beyond basic string checks,
+ * e.g. avoiding empty strings, etc.
+ * Any validation of IP addresses or hostnames is left up to the
+ * rmw implementation, and therefore what is and is not acceptable in these
+ * fields is dependent on it.
+ *
+ * The general expectation, however, is that IP addresses and hostnames are
+ * acceptable.
+ *
+ * The static peers are split by ';' and returned as a list of fixed length
+ * c-strings in the static_peers and static_peers_count fields of the given
+ * discovery_options struct.
+ * Each peer may only be RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH long,
+ * and if it is longer it will be skipped and a warning log message will be
+ * produced.
+ *
+ * \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,182 @@
+// 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/snprintf.h"
+#include "rcutils/split.h"
+#include "rcutils/types/string_array.h"
+
+#include "rcl/error_handling.h"
+#include "rcl/types.h"
+
+#include "rmw/error_handling.h"
+
+#include "./common.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";
+
+#define GET_RMW_DISCOVERY_RANGE(x) \
+  _GET_DEFAULT_DISCOVERY_RANGE(x)
+#define _GET_DEFAULT_DISCOVERY_RANGE(x) \
+  RMW_AUTOMATIC_DISCOVERY_RANGE_ ## x
+
+rcl_ret_t
+rcl_get_automatic_discovery_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 (strcmp(ros_automatic_discovery_range_env_val, "") == 0) {
+#ifdef RCL_DEFAULT_DISCOVERY_RANGE
+    discovery_options->automatic_discovery_range =
+      GET_RMW_DISCOVERY_RANGE(RCL_DEFAULT_DISCOVERY_RANGE);
+#else
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET;
+#endif
+  } 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 if (strcmp(ros_automatic_discovery_range_env_val, "SYSTEM_DEFAULT") == 0) {
+    discovery_options->automatic_discovery_range = RMW_AUTOMATIC_DISCOVERY_RANGE_SYSTEM_DEFAULT;
+  } 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
+const char *
+rcl_automatic_discovery_range_to_string(rmw_automatic_discovery_range_t automatic_discovery_range)
+{
+  switch (automatic_discovery_range) {
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_NOT_SET:
+      return "RMW_AUTOMATIC_DISCOVERY_RANGE_NOT_SET";
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_OFF:
+      return "RMW_AUTOMATIC_DISCOVERY_RANGE_OFF";
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST:
+      return "RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST";
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET:
+      return "RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET";
+    case RMW_AUTOMATIC_DISCOVERY_RANGE_SYSTEM_DEFAULT:
+      return "RMW_AUTOMATIC_DISCOVERY_RANGE_SYSTEM_DEFAULT";
+    default:
+      return NULL;
+  }
+}
+
+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 environment variable '%s': %s",
+      RCL_STATIC_PEERS_ENV_VAR, get_env_error_str);
+    return RCL_RET_ERROR;
+  }
+
+  // The value of the env var should be at least "", even when not set.
+  if (NULL == ros_peers_env_val) {
+    RCL_SET_ERROR_MSG_WITH_FORMAT_STRING(
+      "Environment variable value unexpectedly NULL when checking '%s'",
+      RCL_STATIC_PEERS_ENV_VAR);
+    return RCL_RET_ERROR;
+  }
+
+  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;
+  }
+
+  rmw_ret_t rmw_ret = rmw_discovery_options_init(discovery_options, array.size, allocator);
+  if (RMW_RET_OK != rmw_ret) {
+    RCL_SET_ERROR_MSG(rmw_get_error_string().str);
+    return rcl_convert_rmw_ret_to_rcl_ret(rmw_ret);
+  }
+
+  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;
+    }
+#ifdef _WIN32
+    strncpy_s(
+      discovery_options->static_peers[i].peer_address,
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH,
+      array.data[i],
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH);
+#else
+    strncpy(
+      discovery_options->static_peers[i].peer_address,
+      array.data[i],
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH);
+    discovery_options->static_peers[i].peer_address[
+      RMW_DISCOVERY_OPTIONS_STATIC_PEERS_MAX_LENGTH - 1] = '\0';
+#endif
+  }
+
+  if (RCUTILS_RET_OK != rcutils_string_array_fini(&array)) {
+    RCL_SET_ERROR_MSG(rcutils_get_error_string().str);
+    return RCL_RET_ERROR;
+  }
+
+  return RCL_RET_OK;
+}