Implement RMW init and options (#1)

* Rename implementation info source

* Implement options struct

* Populate options methods

* Implement context struct

* Populate context methods

* Update CMakeLists and package.xml

* Fix implementation typo

* Shift around info printouts

* Remove test member

* Fix typo

* Neaten depends

* Implement allocation for options

* Implement allocation for contexts

* Update RMW_ZENOH_MODE docline

* Update context finalization docstring

* Update case-insensitive string comparison comment

* Replace NULL with nullptr

* Fix options functions to pass tests

Double frees were occuring. Now they're fixed.

* Remove help printouts

* Add is_shutdown to context impl

* Fix context functions to pass tests

* Remove help printouts

* Add doc links

* Reorder includes

* Use deallocator to deallocate context impl struct

* Dynamically allocate and deallocate context impl

* Add missing deallocate calls

* Avoid redundant shutdowns

* Fix code style

rmw_zenoh_cpp/CMakeLists.txt

@@ -13,6 +13,8 @@ find_package(ament_cmake_ros REQUIRED)
 find_package(rcutils REQUIRED)
 find_package(rmw REQUIRED)
 
+find_package(zenoh_ament REQUIRED)
+
 include_directories(include)
 
 add_library(rmw_zenoh_cpp
@@ -25,7 +27,7 @@ add_library(rmw_zenoh_cpp
   src/rmw_graph_info.cpp
   src/rmw_gids.cpp
   src/rmw_guard_conditions.cpp
-  src/rmw_implemtation_info.cpp
+  src/rmw_implementation_info.cpp
   src/rmw_init.cpp
   src/rmw_init_options.cpp
   src/rmw_logging.cpp
@@ -37,8 +39,9 @@ add_library(rmw_zenoh_cpp
   src/rmw_wait_sets.cpp
 )
 ament_target_dependencies(rmw_zenoh_cpp
-  "rcutils"
-  "rmw"
+  rcutils
+  rmw
+  zenoh_ament
 )
 
 configure_rmw_library(rmw_zenoh_cpp)
@@ -51,10 +54,12 @@ ament_export_include_directories(include)
 ament_export_libraries(rmw_zenoh_cpp)
 ament_export_dependencies(rcutils)
 ament_export_dependencies(rmw)
+ament_export_dependencies(zenoh_ament)
 
 register_rmw_implementation(
   "c:rosidl_typesupport_c"
-  "cpp:rosidl_typesupport_cpp")
+  "cpp:rosidl_typesupport_cpp"
+)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

rmw_zenoh_cpp/include/rmw_zenoh_cpp/rmw_context_impl.hpp

@@ -1,10 +1,15 @@
 #ifndef RMW_ZENOH_CPP__RMW_CONTEXT_IMPL_HPP_
 #define RMW_ZENOH_CPP__RMW_CONTEXT_IMPL_HPP_
 
+extern "C"
+{
+  #include "zenoh/zenoh-ffi.h"
+}
+
 struct rmw_context_impl_t
 {
-  /// Test data
-  int context_data;
+  ZNSession * session;
+  bool is_shutdown;
 };
 
 #endif // RMW_ZENOH_CPP__RMW_CONTEXT_IMPL_HPP_

rmw_zenoh_cpp/include/rmw_zenoh_cpp/rmw_init_options_impl.hpp

@@ -0,0 +1,10 @@
+#ifndef RMW_ZENOH_CPP__RMW_INIT_OPTIONS_IMPL_HPP_
+#define RMW_ZENOH_CPP__RMW_INIT_OPTIONS_IMPL_HPP_
+
+struct rmw_init_options_impl_t
+{
+  char * session_locator;  // Zenoh session TCP locator
+  char * mode;  // Zenoh session mode
+};
+
+#endif // RMW_ZENOH_CPP__RMW_INIT_OPTIONS_IMPL_HPP_

rmw_zenoh_cpp/package.xml

@@ -16,11 +16,9 @@
 
   <buildtool_export_depend>ament_cmake</buildtool_export_depend>
 
-  <build_depend>rcutils</build_depend>
-  <build_depend>rmw</build_depend>
-
-  <exec_depend>rcutils</exec_depend>
-  <exec_depend>rmw</exec_depend>
+  <depend>zenoh_ament</depend>
+  <depend>rcutils</depend>
+  <depend>rmw</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

rmw_zenoh_cpp/src/rmw_init.cpp

@@ -1,33 +1,199 @@
-#include "rcutils/logging_macros.h"
+// Doc: http://docs.ros2.org/latest/api/rmw/init_8h.html
+
+#include <cstring>
+
+#include "rmw/impl/cpp/macros.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
+#include "rmw/init.h"
+
+#include "rcutils/logging_macros.h"
+
+#include "rmw_zenoh_cpp/identifier.hpp"
+#include "rmw_zenoh_cpp/rmw_context_impl.hpp"
+#include "rmw_zenoh_cpp/rmw_init_options_impl.hpp"
 
 extern "C"
 {
+#include "zenoh/zenoh-ffi.h"
 
+/// INIT CONTEXT ===============================================================
+// Initialize the middleware with the given options, and yielding an context.
+//
+// rmw_context_t Doc: http://docs.ros2.org/latest/api/rmw/structrmw__context__t.html
+//
+// Starts a new Zenoh session and configures it according to the init options
+// with the following environment variables:
+//  - RMW_ZENOH_SESSION_LOCATOR: Session TCP locator to use
+//  - RMW_ZENOH_MODE: Lets you set the session to be in CLIENT, ROUTER, or PEER mode
+//                    (defaults to PEER)
 rmw_ret_t
 rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
 {
-  (void)options;
-  (void)context;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_init");
-  return RMW_RET_ERROR;
+  // ASSERTIONS ================================================================
+  // Check context
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  if (nullptr != context->implementation_identifier) {
+    RMW_SET_ERROR_MSG("expected a zero-initialized context");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  // Check options
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_FOR_NULL_WITH_MSG(
+    options->implementation_identifier,
+    "expected initialized init options",
+    return RMW_RET_INVALID_ARGUMENT
+  );
+  RMW_CHECK_FOR_NULL_WITH_MSG(
+    options->enclave,
+    "expected non-null enclave",
+    return RMW_RET_INVALID_ARGUMENT
+  );
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    options,
+    options->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+
+  // CLEANUP DEFINITIONS =======================================================
+  // Store a pointer to the context with a custom deleter that zero inits the
+  // context if any initialization steps fail
+  std::unique_ptr<rmw_context_t, void (*)(rmw_context_t *)> clean_when_fail(
+    context,
+    [](rmw_context_t * context)
+    {
+      *context = rmw_get_zero_initialized_context();
+    }
+  );
+
+  rmw_ret_t ret = RMW_RET_OK;
+
+  // INIT CONTEXT ==============================================================
+  context->instance_id = options->instance_id;
+  context->implementation_identifier = eclipse_zenoh_identifier;
+
+  // Populate options
+  context->options = rmw_get_zero_initialized_init_options();
+
+  ret = rmw_init_options_copy(options, &context->options);
+  if (RMW_RET_OK != ret) {
+    if (RMW_RET_OK != rmw_init_options_fini(&context->options)) {
+      RMW_SAFE_FWRITE_TO_STDERR(
+        "'rmw_init_options_fini' failed while being executed due to '"
+        RCUTILS_STRINGIFY(__function__) "' failing.\n");
+    }
+    return ret;
+  }
+
+  // Create implementation specific context
+  rcutils_allocator_t * allocator = &context->options.allocator;
+
+  rmw_context_impl_t * context_impl = static_cast<rmw_context_impl_t *>(
+    allocator->allocate(sizeof(rmw_context_impl_t), allocator->state)
+  );
+
+  if (!context_impl) {
+    RMW_SET_ERROR_MSG("failed to allocate context impl");
+    allocator->deallocate(context_impl, allocator->state);
+
+    return RMW_RET_BAD_ALLOC;
+  }
+
+  // Open configured Zenoh session, then assign it to the context
+  int SESSION_MODE;
+
+  if (strcmp(context->options.impl->mode, "CLIENT") == 0) {
+    SESSION_MODE = CLIENT_MODE;
+  } else if (strcmp(context->options.impl->mode, "CLIENT") == 0) {
+    SESSION_MODE = ROUTER_MODE;
+  } else {
+    SESSION_MODE = PEER_MODE;
+  }
+
+  ZNSession * s = zn_open(SESSION_MODE,
+                          context->options.impl->session_locator,
+                          0);
+
+  if (s == nullptr) {
+    RMW_SET_ERROR_MSG("failed to create Zenoh session when starting context");
+    allocator->deallocate(context_impl, allocator->state);
+    return RMW_RET_ERROR;
+  } else {
+    context_impl->session = s;
+    context_impl->is_shutdown = false;
+  }
+
+  // CLEANUP IF PASSED =========================================================
+  context->impl = context_impl;
+  clean_when_fail.release();
+  return RMW_RET_OK;
 }
 
+/// SHUTDOWN CONTEXT ===========================================================
+// Shutdown the middleware for a given context.
+//
+// In this case, closes the associated Zenoh session.
 rmw_ret_t
 rmw_shutdown(rmw_context_t * context)
 {
-  (void)context;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_shutdown");
-  return RMW_RET_ERROR;
+  // Assertions
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_FOR_NULL_WITH_MSG(
+    context->impl,
+    "expected initialized context",
+    return RMW_RET_INVALID_ARGUMENT
+  );
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    context,
+    context->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+
+  // Close Zenoh session
+  if (context->impl->is_shutdown == false)
+  {
+    zn_close(context->impl->session);
+    context->impl->is_shutdown = true;
+  }
+
+  return RMW_RET_OK;
 }
 
+/// FINALIZE CONTEXT ===========================================================
+// Finalize a context. (Cleanup and deallocation.)
 rmw_ret_t
 rmw_context_fini(rmw_context_t * context)
 {
-  (void)context;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_context_fini");
-  return RMW_RET_ERROR;
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_FOR_NULL_WITH_MSG(
+    context->impl,
+    "expected initialized context",
+    return RMW_RET_INVALID_ARGUMENT
+  );
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    context,
+    context->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+  if (!context->impl->is_shutdown) {
+    RCUTILS_SET_ERROR_MSG("context has not been shutdown");
+    return RMW_RET_INVALID_ARGUMENT;
+  }
+
+  rcutils_allocator_t * allocator = &context->options.allocator;
+
+  // Deallocate implementation specific members
+  allocator->deallocate(context->impl->session, allocator->state);
+  allocator->deallocate(context->impl, allocator->state);
+
+  // Reset context
+  *context = rmw_get_zero_initialized_context();
+
+  return RMW_RET_OK;
 }
 
 } // extern "C"