[rmw_cyclonedds] Improve handling of dynamic discovery (#429)

* First go at getting Cyclone to work with the new discovery configuration params Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net> * Adopt suggestions from Erik Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net> * most tests passing Signed-off-by: Michael X. Grey <grey@openrobotics.org> * Revised, recommended behavior for discovery Signed-off-by: Michael X. Grey <grey@openrobotics.org> * Update to latest rmw API Signed-off-by: Michael X. Grey <grey@openrobotics.org> * C++ 17, debug log, and linters Signed-off-by: Shane Loretz <sloretz@google.com> * NOT_SET and SYSTEM_DEFAULT values Signed-off-by: Shane Loretz <sloretz@google.com> * Remove TODO that won't be fixed Signed-off-by: Shane Loretz <sloretz@google.com> * Make linters happy Signed-off-by: Shane Loretz <sloretz@google.com> * Call rmw_discovery_options_init() Signed-off-by: Shane Loretz <sloretz@google.com> * Set max participant index to largest value possible Signed-off-by: Shane Loretz <sloretz@google.com> * Set ParticipantIndex to none when OFF Signed-off-by: Shane Loretz <sloretz@google.com> * Error when range is an invalid value Signed-off-by: Shane Loretz <sloretz@google.com> * Use localhost has peer address Signed-off-by: Shane Loretz <sloretz@google.com> * Set MaxAutoParticipantIndex to 32 Signed-off-by: Shane Loretz <sloretz@google.com> --------- Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net> Signed-off-by: Michael X. Grey <grey@openrobotics.org> Signed-off-by: Shane Loretz <sloretz@google.com> Co-authored-by: Michael X. Grey <grey@openrobotics.org> Co-authored-by: Shane Loretz <sloretz@google.com>
ros2 · Apr 8, 2023 · 6510ad2 · 6510ad2
1 parent fec9b04
commit 6510ad2
Show file tree

Hide file tree

Showing 2 changed files with 141 additions and 26 deletions.
diff --git a/rmw_cyclonedds_cpp/CMakeLists.txt b/rmw_cyclonedds_cpp/CMakeLists.txt
@@ -18,7 +18,7 @@ project(rmw_cyclonedds_cpp)
 
 # Default to C++14
 if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD 17)
 endif()
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")

diff --git a/rmw_cyclonedds_cpp/src/rmw_node.cpp b/rmw_cyclonedds_cpp/src/rmw_node.cpp
@@ -36,10 +36,12 @@
 #include "rcutils/filesystem.h"
 #include "rcutils/format_string.h"
 #include "rcutils/logging_macros.h"
+#include "rcutils/process.h"
 #include "rcutils/strdup.h"
 
 #include "rmw/allocators.h"
 #include "rmw/convert_rcutils_ret_to_rmw_ret.h"
+#include "rmw/discovery_options.h"
 #include "rmw/error_handling.h"
 #include "rmw/event.h"
 #include "rmw/features.h"
@@ -257,16 +259,18 @@ struct CddsDomain
      There are a few issues with the current support for creating domains explicitly in
      Cyclone, fixing those might relax alter or relax some of the above. */
 
-  bool localhost_only;
+  rmw_discovery_options_t discovery_options;
   uint32_t refcount;
 
   /* handle of the domain entity */
   dds_entity_t domain_handle;
 
   /* Default constructor so operator[] can be safely be used to look one up */
   CddsDomain()
-  : localhost_only(false), refcount(0), domain_handle(0)
-  {}
+  : refcount(0), domain_handle(0)
+  {
+    discovery_options = rmw_get_zero_initialized_discovery_options();
+  }
 
   ~CddsDomain()
   {}
@@ -783,10 +787,11 @@ extern "C" rmw_ret_t rmw_init_options_init(
   init_options->allocator = allocator;
   init_options->impl = nullptr;
   init_options->localhost_only = RMW_LOCALHOST_ONLY_DEFAULT;
+  init_options->discovery_options = rmw_get_zero_initialized_discovery_options(),
   init_options->domain_id = RMW_DEFAULT_DOMAIN_ID;
   init_options->enclave = NULL;
   init_options->security_options = rmw_get_zero_initialized_security_options();
-  return RMW_RET_OK;
+  return rmw_discovery_options_init(&(init_options->discovery_options), 0, &allocator);
 }
 
 extern "C" rmw_ret_t rmw_init_options_copy(const rmw_init_options_t * src, rmw_init_options_t * dst)
@@ -1093,39 +1098,147 @@ static rmw_ret_t discovery_thread_stop(rmw_dds_common::Context & common_context)
   return RMW_RET_OK;
 }
 
-static bool check_create_domain(dds_domainid_t did, rmw_localhost_only_t localhost_only_option)
+static bool check_create_domain(dds_domainid_t did, rmw_discovery_options_t * discovery_options)
 {
-  const bool localhost_only = (localhost_only_option == RMW_LOCALHOST_ONLY_ENABLED);
   std::lock_guard<std::mutex> lock(gcdds().domains_lock);
-  /* return true: n_nodes incremented, localhost_only set correctly, domain exists
+  /* return true: n_nodes incremented, discovery params set correctly, domain exists
      "      false: n_nodes unchanged, domain left intact if it already existed */
   CddsDomain & dom = gcdds().domains[did];
   if (dom.refcount != 0) {
-    /* Localhost setting must match */
-    if (localhost_only == dom.localhost_only) {
+    /* Discovery parameters must match */
+    bool options_equal = false;
+    const auto rc =
+      rmw_discovery_options_equal(discovery_options, &dom.discovery_options, &options_equal);
+    if (RMW_RET_OK != rc) {
+      RCUTILS_LOG_ERROR_NAMED(
+        "rmw_cyclonedds_cpp",
+        "check_create_domain: unable to check if discovery options are equal: %i",
+        rc);
+      return false;
+    }
+    if (options_equal) {
       dom.refcount++;
       return true;
     } else {
       RCUTILS_LOG_ERROR_NAMED(
         "rmw_cyclonedds_cpp",
-        "rmw_create_node: attempt at creating localhost-only and non-localhost-only nodes "
-        "in the same domain");
+        "check_create_domain: attempt at creating nodes in the same domain with different "
+        "discovery parameters");
       return false;
     }
   } else {
     dom.refcount = 1;
-    dom.localhost_only = localhost_only;
-
-    /* Localhost-only: set network interface address (shortened form of config would be
-       possible, too, but I think it is clearer to spell it out completely).  Empty
-       configuration fragments are ignored, so it is safe to unconditionally append a
-       comma. */
-    std::string config =
-      localhost_only ?
-      "<CycloneDDS><Domain><General><Interfaces><NetworkInterface address=\"127.0.0.1\"/>"
-      "</Interfaces></General></Domain></CycloneDDS>,"
-      :
-      "";
+    dom.discovery_options = *discovery_options;
+
+    bool add_localhost_as_static_peer;
+    bool add_static_peers;
+    bool disable_multicast;
+
+    switch (discovery_options->automatic_discovery_range) {
+      case RMW_AUTOMATIC_DISCOVERY_RANGE_NOT_SET:
+        RMW_SET_ERROR_MSG("automatic discovery range must be set");
+        return false;
+        break;
+      case RMW_AUTOMATIC_DISCOVERY_RANGE_SUBNET:
+        add_localhost_as_static_peer = false;
+        add_static_peers = true;
+        disable_multicast = false;
+        break;
+      case RMW_AUTOMATIC_DISCOVERY_RANGE_SYSTEM_DEFAULT:
+        /* Avoid changing DDS discovery options*/
+        add_localhost_as_static_peer = false;
+        add_static_peers = false;
+        disable_multicast = false;
+        if (discovery_options->static_peers_count > 0) {
+          RCUTILS_LOG_WARN_NAMED(
+            "rmw_cyclonedds_cpp",
+            "check_create_domain: %lu static peers were specified, but discovery is "
+            "set to use the RMW implementation default, so these static peers will be ignored.",
+            discovery_options->static_peers_count);
+        }
+        break;
+      case RMW_AUTOMATIC_DISCOVERY_RANGE_LOCALHOST:
+        /* Automatic discovery on localhost only */
+        add_localhost_as_static_peer = true;
+        add_static_peers = true;
+        disable_multicast = true;
+        break;
+      case RMW_AUTOMATIC_DISCOVERY_RANGE_OFF:
+        /* Automatic discovery off: disable multicast entirely. */
+        add_localhost_as_static_peer = false;
+        add_static_peers = false;
+        disable_multicast = true;
+        if (discovery_options->static_peers_count > 0) {
+          RCUTILS_LOG_WARN_NAMED(
+            "rmw_cyclonedds_cpp",
+            "check_create_domain: %lu static peers were specified, but discovery is "
+            "turned off, so these static peers will be ignored.",
+            discovery_options->static_peers_count);
+        }
+        break;
+      default:
+        RMW_SET_ERROR_MSG("automatic_discovery_range is an unknown value");
+        return false;
+        break;
+    }
+
+    std::string config;
+    if (
+      add_localhost_as_static_peer ||
+      add_static_peers ||
+      disable_multicast)
+    {
+      config = "<CycloneDDS><Domain>";
+
+      if (disable_multicast) {
+        config += "<General><AllowMulticast>false</AllowMulticast></General>";
+      }
+
+      const bool discovery_off =
+        disable_multicast && !add_localhost_as_static_peer && !add_static_peers;
+      if (discovery_off) {
+        /* This means we have an OFF range, so we should use the domain tag to
+          block all attemtps at automatic discovery. Another participant would
+          need to use this exact same domain tag, down to the PID, to discover
+          the endpoints of this node.
+
+          Setting ParticipantIndex to none eliminates the 119 limit on the number
+          of participants on a machine.
+          */
+        config += "<Discovery><ParticipantIndex>none</ParticipantIndex>";
+        config += "<Tag>ros_discovery_off_" + std::to_string(rcutils_get_pid()) + "</Tag>";
+      } else {
+        config += "<Discovery><ParticipantIndex>auto</ParticipantIndex>";
+        // This controls the number of participants that can be discovered on a single host,
+        // which is roughly equivalent to the number of ROS 2 processes.
+        // If it's too small then we won't connect to all participants.
+        // If it's too large then we will send a lot of announcement traffic.
+        // The default number here is picked arbitrarily.
+        config += "<MaxAutoParticipantIndex>32</MaxAutoParticipantIndex>";
+      }
+
+      if (  // NOLINT
+        (add_static_peers && discovery_options->static_peers_count > 0) ||
+        add_localhost_as_static_peer)
+      {
+        config += "<Peers>";
+
+        if (add_localhost_as_static_peer) {
+          config += "<Peer address=\"localhost\"/>";
+        }
+
+        for (size_t ii = 0; ii < discovery_options->static_peers_count; ++ii) {
+          config += "<Peer address=\"";
+          config += discovery_options->static_peers[ii].peer_address;
+          config += "\"/>";
+        }
+        config += "</Peers>";
+      }
+
+      /* NOTE: Empty configuration fragments are ignored, so it is safe to
+        unconditionally append a comma. */
+      config += "</Discovery></Domain></CycloneDDS>,";
+    }
 
     /* Emulate default behaviour of Cyclone of reading CYCLONEDDS_URI */
     const char * get_env_error;
@@ -1141,6 +1254,8 @@ static bool check_create_domain(dds_domainid_t did, rmw_localhost_only_t localho
       return false;
     }
 
+    RCUTILS_LOG_DEBUG_NAMED("rmw_cyclonedds_cpp", "Config XML is %s", config.c_str());
+
     if ((dom.domain_handle = dds_create_domain(did, config.c_str())) < 0) {
       RCUTILS_LOG_ERROR_NAMED(
         "rmw_cyclonedds_cpp",
@@ -1240,7 +1355,7 @@ rmw_context_impl_s::init(rmw_init_options_t * options, size_t domain_id)
     version of dds_create_domain that doesn't return a handle.  */
   this->domain_id = static_cast<dds_domainid_t>(domain_id);
 
-  if (!check_create_domain(this->domain_id, options->localhost_only)) {
+  if (!check_create_domain(this->domain_id, &options->discovery_options)) {
     return RMW_RET_ERROR;
   }
 
@@ -1498,7 +1613,7 @@ extern "C" rmw_ret_t rmw_init(const rmw_init_options_t * options, rmw_context_t
 
   if (options->domain_id >= UINT32_MAX && options->domain_id != RMW_DEFAULT_DOMAIN_ID) {
     RCUTILS_LOG_ERROR_NAMED(
-      "rmw_cyclonedds_cpp", "rmw_create_node: domain id out of range");
+      "rmw_cyclonedds_cpp", "rmw_init: domain id out of range");
     return RMW_RET_INVALID_ARGUMENT;
   }