Implement ancillary methods and stubs (#5)

* Implement guard condition

* Implement wait sets

* Add publisher qos stub

* Add dummy service creation

* Implement node graph guard condition getter

* Add publisher event stub

* Implement publisher GID getter

* Remove help printout for implementation identifier

* Implement subscription event stub

* Fix service creation stub

* Fix subscriber creation stub

* Clarify stubs

* Implement event stubs

* Change header guards to match convention

* Remove redundant stub comment

* Use RMW_CHECK_ARGUMENT_FOR_NULL

* Use rmw_guard_condition_allocate

* Rearrange includes

* Explicitly call struct constructor with placement new

rmw_zenoh_cpp/src/rmw_event.cpp

@@ -1,18 +1,26 @@
+// Docs: http://docs.ros2.org/latest/api/rmw/event_8h.html
+
 #include "rcutils/logging_macros.h"
+
+#include "rmw/impl/cpp/macros.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "rmw/event.h"
 
+#include "rmw_zenoh_cpp/identifier.hpp"
+
 extern "C"
 {
-
+// STUB
 rmw_ret_t
 rmw_take_event(const rmw_event_t * event_handle, void * event_info, bool * taken)
 {
   (void)event_handle;
   (void)event_info;
-  (void)taken;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_take_event");
+
+  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_take_event (STUB)");
+  *taken = true;
+
   return RMW_RET_OK;
 }
 
@@ -22,11 +30,30 @@ rmw_publisher_event_init(
   const rmw_publisher_t * publisher,
   rmw_event_type_t event_type)
 {
-  (void)event;
-  (void)publisher;
-  (void)event_type;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_publisher_event_init");
-  return RMW_RET_ERROR;
+  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_publisher_event_init (STUB)");
+
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(event, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(event_type, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    publisher,
+    publisher->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+
+  // TODO(CH3) NOTE(CH3): Check if event type is supported
+  // Most likely no. It seems to be a DDS QoS specific thing
+  // if (!internal::is_event_supported(event_type)) {
+  //   RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier);
+  //   return RMW_RET_UNSUPPORTED;
+  // }
+
+  event->implementation_identifier = publisher->implementation_identifier;
+  event->data = publisher->data;
+  event->event_type = event_type;
+
+  return RMW_RET_OK;
 }
 
 rmw_ret_t
@@ -35,11 +62,30 @@ rmw_subscription_event_init(
   const rmw_subscription_t * subscription,
   rmw_event_type_t event_type)
 {
-  (void)event;
-  (void)subscription;
-  (void)event_type;
   RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_subscription_event_init");
-  return RMW_RET_ERROR;
+
+  RMW_CHECK_ARGUMENT_FOR_NULL(subscription, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(event, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(event_type, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    subscription,
+    subscription->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+
+  // TODO(CH3) NOTE(CH3): Check if event type is supported
+  // Most likely no. It seems to be a DDS QoS specific thing
+  // if (!internal::is_event_supported(event_type)) {
+  //   RMW_SET_ERROR_MSG_WITH_FORMAT_STRING("provided event_type is not supported by %s", identifier);
+  //   return RMW_RET_UNSUPPORTED;
+  // }
+
+  event->implementation_identifier = subscription->implementation_identifier;
+  event->data = subscription->data;
+  event->event_type = event_type;
+
+  return RMW_RET_OK;
 }
 
 } // extern "C"

rmw_zenoh_cpp/src/rmw_gids.cpp

@@ -1,19 +1,39 @@
 #include "rcutils/logging_macros.h"
+
+#include "rmw/topic_endpoint_info_array.h"
+#include "rmw/impl/cpp/macros.hpp"
 #include "rmw/error_handling.h"
-#include "rmw/rmw.h"
 #include "rmw/names_and_types.h"
-#include "rmw/topic_endpoint_info_array.h"
+#include "rmw/rmw.h"
+
+#include "rmw_zenoh_cpp/zenoh_pubsub.hpp"
+#include "rmw_zenoh_cpp/identifier.hpp"
 
 extern "C"
 {
-
+/// GET PUBLISHER GID ==========================================================
+// rmw_gid_t Doc: http://docs.ros2.org/latest/api/rmw/structrmw__gid__t.html
 rmw_ret_t
 rmw_get_gid_for_publisher(const rmw_publisher_t * publisher, rmw_gid_t * gid)
 {
-  (void)publisher;
-  (void)gid;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_get_gid_for_publisher");
-  return RMW_RET_ERROR;
+  RMW_CHECK_ARGUMENT_FOR_NULL(publisher, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    publisher,
+    publisher->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  );
+  RMW_CHECK_ARGUMENT_FOR_NULL(gid, RMW_RET_INVALID_ARGUMENT);
+
+  // Copy size_t to gid member
+  memset(gid->data, 0, sizeof(gid->data));
+  memcpy(
+    gid->data,
+    &static_cast<rmw_publisher_data_t *>(publisher->data)->zn_topic_id_,
+    sizeof(static_cast<rmw_publisher_data_t *>(publisher->data)->zn_topic_id_)
+  );
+
+  return RMW_RET_OK;
 }
 
 rmw_ret_t

rmw_zenoh_cpp/src/rmw_guard_conditions.cpp

@@ -1,41 +1,71 @@
-#include "rcutils/logging_macros.h"
+// Doc: http://docs.ros2.org/latest/api/rmw/rmw_8h.html
+// Edited from: https://github.com/ros2/rmw_fastrtps/blob/master/rmw_fastrtps_shared_cpp/src/rmw_guard_condition.cpp
+// Under the Apache 2.0 license
+
+#include "rmw/impl/cpp/macros.hpp"
 #include "rmw/error_handling.h"
 #include "rmw/rmw.h"
 #include "rmw/event.h"
 
+#include "rcutils/logging_macros.h"
+
+#include "rmw_zenoh_cpp/identifier.hpp"
+#include "types/guard_condition.hpp"
+
 extern "C"
 {
-
+/// CREATE GUARD CONDITION =====================================================
+// Create a guard condition and return a handle to that guard condition.
 rmw_guard_condition_t *
 rmw_create_guard_condition(rmw_context_t * context)
 {
-  (void)context;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_create_guard_condition");
-  return nullptr;
+  RMW_CHECK_ARGUMENT_FOR_NULL(context, nullptr);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    context,
+    context->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return nullptr
+  );
+
+  // NOTE(CH3): Unfortunately we can't do custom allocation here because the destruction method
+  // does not pass in a context from which we can draw an allocator from
+  // TODO(CH3): Once https://github.com/ros2/rmw/issues/260 gets resolved, use an allocator instead
+  rmw_guard_condition_t * guard_condition_handle = rmw_guard_condition_allocate();
+  guard_condition_handle->implementation_identifier = eclipse_zenoh_identifier;
+  guard_condition_handle->data = new GuardCondition();
+
+  return guard_condition_handle;
 }
 
+/// DESTROY GUARD CONDITION ====================================================
+// Finalize a given guard condition handle, reclaim the resources, and deallocate the handle.
 rmw_ret_t
 rmw_destroy_guard_condition(rmw_guard_condition_t * guard_condition_handle)
 {
-  (void)guard_condition_handle;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_destroy_guard_condition");
-  return RMW_RET_OK;
+  if (guard_condition_handle) {
+    delete static_cast<GuardCondition *>(guard_condition_handle->data);
+    delete guard_condition_handle;
+    return RMW_RET_OK;
+  }
+
+  return RMW_RET_ERROR;
 }
 
+/// TRIGGER GUARD CONDITION ====================================================
 rmw_ret_t
 rmw_trigger_guard_condition(const rmw_guard_condition_t * guard_condition_handle)
 {
-  (void)guard_condition_handle;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_trigger_guard_condition");
-  return RMW_RET_OK;
-}
+  RMW_CHECK_ARGUMENT_FOR_NULL(guard_condition_handle, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_TYPE_IDENTIFIERS_MATCH(
+    guard_condition_handle,
+    guard_condition_handle->implementation_identifier,
+    eclipse_zenoh_identifier,
+    return RMW_RET_INCORRECT_RMW_IMPLEMENTATION
+  )
 
-const rmw_guard_condition_t *
-rmw_node_get_graph_guard_condition(const rmw_node_t * node)
-{
-  (void)node;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_node_get_graph_guard_condition");
-  return nullptr;
+  static_cast<GuardCondition *>(guard_condition_handle->data)->trigger();
+
+  return RMW_RET_OK;
 }
 
 } // extern "C"

rmw_zenoh_cpp/src/rmw_implementation_info.cpp

@@ -10,7 +10,6 @@ extern "C"
 const char *
 rmw_get_implementation_identifier()
 {
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_get_implementation_identifier");
   return eclipse_zenoh_identifier;
 }
 
@@ -23,4 +22,3 @@ rmw_get_serialization_format()
 
 
 } // extern "C"
-

rmw_zenoh_cpp/src/rmw_init.cpp

@@ -32,14 +32,14 @@ rmw_init(const rmw_init_options_t * options, rmw_context_t * context)
 {
   // ASSERTIONS ================================================================
   // Check context
-  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_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_ARGUMENT_FOR_NULL(options, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_FOR_NULL_WITH_MSG(
     options->implementation_identifier,
     "expected initialized init options",
@@ -139,7 +139,7 @@ rmw_ret_t
 rmw_shutdown(rmw_context_t * context)
 {
   // Assertions
-  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_FOR_NULL_WITH_MSG(
     context->impl,
     "expected initialized context",
@@ -167,7 +167,7 @@ rmw_shutdown(rmw_context_t * context)
 rmw_ret_t
 rmw_context_fini(rmw_context_t * context)
 {
-  RCUTILS_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
+  RMW_CHECK_ARGUMENT_FOR_NULL(context, RMW_RET_INVALID_ARGUMENT);
   RMW_CHECK_FOR_NULL_WITH_MSG(
     context->impl,
     "expected initialized context",

rmw_zenoh_cpp/src/rmw_node.cpp

@@ -214,4 +214,12 @@ rmw_destroy_node(rmw_node_t * node)
   return RMW_RET_OK;
 }
 
+/// GET NODE GRAPH CONDITION ===================================================
+const rmw_guard_condition_t *
+rmw_node_get_graph_guard_condition(const rmw_node_t * node)
+{
+  RMW_CHECK_ARGUMENT_FOR_NULL(node->data, nullptr);
+  return static_cast<rmw_node_impl_t *>(node->data)->graph_guard_condition_;
+}
+
 } // extern "C"

rmw_zenoh_cpp/src/rmw_publisher.cpp

@@ -110,13 +110,15 @@ rmw_publisher_count_matched_subscriptions(const rmw_publisher_t * publisher, siz
   return RMW_RET_ERROR;
 }
 
+// STUB
 rmw_ret_t
 rmw_publisher_get_actual_qos(const rmw_publisher_t * publisher, rmw_qos_profile_t * qos_profile)
 {
   (void)publisher;
   (void)qos_profile;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_publisher_get_actual_qos");
-  return RMW_RET_ERROR;
+  // RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_publisher_get_actual_qos");
+  // return RMW_RET_ERROR;
+  return RMW_RET_OK;
 }
 
 rmw_ret_t

rmw_zenoh_cpp/src/rmw_services.cpp

@@ -54,6 +54,7 @@ rmw_take_response(
   return RMW_RET_ERROR;
 }
 
+// STUB
 rmw_service_t *
 rmw_create_service(
   const rmw_node_t * node,
@@ -65,19 +66,24 @@ rmw_create_service(
   (void)type_support;
   (void)service_name;
   (void)qos_profile;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_create_service");
-  return nullptr;
+  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_create_service (STUB)");
+
+  // TODO(CH3): Properly do allocations
+  rmw_service_t * dummy = new rmw_service_t;
+  return dummy;
 }
 
+// STUB
 rmw_ret_t
 rmw_destroy_service(rmw_node_t * node, rmw_service_t * service)
 {
   (void)node;
   (void)service;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_destroy_service");
-  return RMW_RET_ERROR;
+  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_destroy_service (STUB)");
+  return RMW_RET_OK;
 }
 
+// STUB
 rmw_ret_t
 rmw_take_request(
   const rmw_service_t * service,
@@ -89,8 +95,9 @@ rmw_take_request(
   (void)request_header;
   (void)ros_request;
   (void)taken;
-  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_take_request");
-  return RMW_RET_ERROR;
+  RCUTILS_LOG_INFO_NAMED("rmw_zenoh_cpp", "rmw_take_request (STUB)");
+  // return RMW_RET_ERROR;
+  return RMW_RET_OK;
 }
 
 rmw_ret_t