Implement services and rmw_wait

rmw_zenoh_cpp/CMakeLists.txt

@@ -42,12 +42,16 @@ add_library(rmw_zenoh_cpp
   src/rmw_publish.cpp
   src/rmw_publisher.cpp
   src/rmw_serialization.cpp
-  src/rmw_services.cpp
+  src/rmw_service.cpp
+  src/rmw_client.cpp
   src/rmw_subscriber.cpp
   src/rmw_wait_sets.cpp
 
   src/impl/identifier.cpp
+  src/impl/wait_impl.cpp
   src/impl/pubsub_impl.cpp
+  src/impl/service_impl.cpp
+  src/impl/client_impl.cpp
   src/impl/type_support_common.cpp
 )
 target_link_libraries(rmw_zenoh_cpp

rmw_zenoh_cpp/include/rmw_zenoh_cpp/ServiceTypeSupport.hpp

@@ -0,0 +1,31 @@
+#ifndef RMW_ZENOH_CPP__SERVICETYPESUPPORT_HPP_
+#define RMW_ZENOH_CPP__SERVICETYPESUPPORT_HPP_
+
+#include "rosidl_typesupport_zenoh_cpp/message_type_support.h"
+#include "rosidl_typesupport_zenoh_cpp/service_type_support.h"
+#include "TypeSupport.hpp"
+
+namespace rmw_zenoh_cpp
+{
+
+class ServiceTypeSupport : public TypeSupport
+{
+protected:
+  ServiceTypeSupport();
+};
+
+class RequestTypeSupport : public ServiceTypeSupport
+{
+public:
+  explicit RequestTypeSupport(const service_type_support_callbacks_t * members);
+};
+
+class ResponseTypeSupport : public ServiceTypeSupport
+{
+public:
+  explicit ResponseTypeSupport(const service_type_support_callbacks_t * members);
+};
+
+}  // namespace rmw_zenoh_cpp
+
+#endif  // RMW_ZENOH_CPP__SERVICETYPESUPPORT_HPP_

rmw_zenoh_cpp/include/rmw_zenoh_cpp/TypeSupport.hpp

@@ -12,6 +12,7 @@
 
 namespace rmw_zenoh_cpp
 {
+
 class TypeSupport
 {
 public:

rmw_zenoh_cpp/src/impl/client_impl.cpp

@@ -0,0 +1,61 @@
+extern "C"
+{
+  #include "zenoh/zenoh-ffi.h"
+}
+
+#include <iostream>
+#include <mutex>
+
+#include "rmw_zenoh_cpp/TypeSupport.hpp"
+#include "client_impl.hpp"
+
+std::mutex response_callback_mutex;
+std::mutex query_callback_mutex;
+
+// Static sequence ID
+int64_t rmw_client_data_t::sequence_id = -1;
+
+// Static response ROS message map
+std::unordered_map<std::string, std::vector<unsigned char> >
+  rmw_client_data_t::zn_response_messages_;
+
+// Static availability query Zenoh response set
+std::unordered_set<std::string> rmw_client_data_t::zn_availability_query_responses_;
+
+/// ZENOH RESPONSE SUBSCRIPTION CALLBACK =======================================
+void rmw_client_data_t::zn_response_sub_callback(const zn_sample * sample) {
+  // Prevent race conditions...
+  std::lock_guard<std::mutex> guard(response_callback_mutex);
+
+  // NOTE(CH3): We unfortunately have to do this copy construction since we shouldn't be using
+  // char * as keys to the unordered_map
+  std::string key(sample->key.val, sample->key.len);
+
+  // Vector to store the byte array (so we have a copyable type instead of a pointer)
+  std::vector<unsigned char> byte_vec(sample->value.val, sample->value.val + sample->value.len);
+
+  // Fill the static response message map with the latest received message
+  //
+  // NOTE(CH3): This means that the queue size for each topic is ONE for now!!
+  // So this might break if a service is being spammed.
+  // TODO(CH3): Implement queuing logic
+  rmw_client_data_t::zn_response_messages_[key] = std::vector<unsigned char>(byte_vec);
+}
+
+/// ZENOH SERVICE AVAILABILITY QUERY CALLBACK ==================================
+void rmw_client_data_t::zn_service_availability_query_callback(const zn_source_info * info,
+                                                               const zn_sample * sample) {
+  // Prevent race conditions...
+  std::lock_guard<std::mutex> guard(query_callback_mutex);
+
+  // NOTE(CH3): We unfortunately have to do this copy construction since we shouldn't be using
+  // char * as keys to the unordered_map
+  std::string key(sample->key.val, sample->key.len);
+
+  // Insert if key not found in query response set
+  if (rmw_client_data_t::zn_availability_query_responses_.find(key)
+    == rmw_client_data_t::zn_availability_query_responses_.end())
+  {
+    rmw_client_data_t::zn_availability_query_responses_.insert(key);
+  }
+}

rmw_zenoh_cpp/src/impl/client_impl.hpp

@@ -0,0 +1,58 @@
+#ifndef RMW_ZENOH_CPP__ZENOH_CLIENT_HPP_
+#define RMW_ZENOH_CPP__ZENOH_CLIENT_HPP_
+
+#include <unordered_map>
+#include <unordered_set>
+#include <utility>
+#include <string>
+#include <vector>
+
+extern "C"
+{
+  #include "zenoh/zenoh-ffi.h"
+}
+
+#include "rmw/rmw.h"
+#include "rmw_zenoh_cpp/TypeSupport.hpp"
+
+struct rmw_client_data_t
+{
+  /// STATIC MEMBERS ===========================================================
+  static void zn_response_sub_callback(const zn_sample * sample);
+  static void zn_service_availability_query_callback(
+    const zn_source_info * info, const zn_sample * sample
+  );
+
+  // Sequence id
+  static int64_t sequence_id;
+
+  // Serialized ROS response messages
+  static std::unordered_map<std::string, std::vector<unsigned char> > zn_response_messages_;
+
+  // Availability query Zenoh responses
+  static std::unordered_set<std::string> zn_availability_query_responses_;
+
+  /// TYPE SUPPORT =============================================================
+  const void * request_type_support_impl_;
+  const void * response_type_support_impl_;
+  const char * typesupport_identifier_;
+
+  rmw_zenoh_cpp::TypeSupport * request_type_support_;
+  rmw_zenoh_cpp::TypeSupport * response_type_support_;
+
+  /// ZENOH ====================================================================
+  ZNSession * zn_session_;
+
+  // Response Sub
+  const char * zn_response_topic_key_;
+  ZNSubscriber * zn_response_subscriber_;
+
+  // Request Pub
+  const char * zn_request_topic_key_;
+  size_t zn_request_topic_id_;
+
+  /// ROS ======================================================================
+  const rmw_node_t * node_;
+};
+
+#endif // RMW_ZENOH_CPP__ZENOH_CLIENT_HPP_

rmw_zenoh_cpp/src/impl/pubsub_impl.cpp

@@ -3,14 +3,18 @@ extern "C"
   #include "zenoh/zenoh-ffi.h"
 }
 
-#include "rmw_zenoh_cpp/TypeSupport.hpp"
-#include "pubsub_impl.hpp"
 #include <iostream>
 #include <mutex>
 
+#include "rmw_zenoh_cpp/TypeSupport.hpp"
+#include "pubsub_impl.hpp"
+
 std::mutex sub_callback_mutex;
 
-// Declaring static members
+// Static message map
+std::unordered_map<std::string, std::vector<unsigned char> >
+  rmw_subscription_data_t::zn_messages_;
+
 void rmw_subscription_data_t::zn_sub_callback(const zn_sample * sample) {
   // Prevent race conditions...
   std::lock_guard<std::mutex> guard(sub_callback_mutex);
@@ -22,8 +26,10 @@ void rmw_subscription_data_t::zn_sub_callback(const zn_sample * sample) {
   // Vector to store the byte array (so we have a copyable type instead of a pointer)
   std::vector<unsigned char> byte_vec(sample->value.val, sample->value.val + sample->value.len);
 
+  // Fill the static message map with the latest received message
+  //
+  // NOTE(CH3): This means that the queue size for each topic is ONE for now!!
+  // So this might break if a topic is being spammed.
+  // TODO(CH3): Implement queuing logic
   rmw_subscription_data_t::zn_messages_[key] = std::vector<unsigned char>(byte_vec);
 }
-
-std::unordered_map<std::string, std::vector<unsigned char> >
-  rmw_subscription_data_t::zn_messages_;

rmw_zenoh_cpp/src/impl/pubsub_impl.hpp

@@ -35,7 +35,6 @@ struct rmw_publisher_data_t
 // Functionally a struct. But with a method for handling incoming Zenoh messages
 struct rmw_subscription_data_t
 {
-  // TODO(CH3): If needed, implement lock guards/mutexes to prevent race conditions
   static void zn_sub_callback(const zn_sample * sample);
 
   // Map of Zenoh topic key expression to latest serialized ROS messages

rmw_zenoh_cpp/src/impl/service_impl.cpp

@@ -0,0 +1,35 @@
+extern "C"
+{
+  #include "zenoh/zenoh-ffi.h"
+}
+
+#include <iostream>
+#include <mutex>
+
+#include "rmw_zenoh_cpp/TypeSupport.hpp"
+#include "service_impl.hpp"
+
+std::mutex request_callback_mutex;
+
+// Static request message map
+std::unordered_map<std::string, std::vector<unsigned char> >
+  rmw_service_data_t::zn_request_messages_;
+
+void rmw_service_data_t::zn_request_sub_callback(const zn_sample * sample) {
+  // Prevent race conditions...
+  std::lock_guard<std::mutex> guard(request_callback_mutex);
+
+  // NOTE(CH3): We unfortunately have to do this copy construction since we shouldn't be using
+  // char * as keys to the unordered_map
+  std::string key(sample->key.val, sample->key.len);
+
+  // Vector to store the byte array (so we have a copyable type instead of a pointer)
+  std::vector<unsigned char> byte_vec(sample->value.val, sample->value.val + sample->value.len);
+
+  // Fill the static request message map with the latest received message
+  //
+  // NOTE(CH3): This means that the queue size for each topic is ONE for now!!
+  // So this might break if a service is being spammed.
+  // TODO(CH3): Implement queuing logic
+  rmw_service_data_t::zn_request_messages_[key] = std::vector<unsigned char>(byte_vec);
+}

rmw_zenoh_cpp/src/impl/service_impl.hpp

@@ -0,0 +1,49 @@
+#ifndef RMW_ZENOH_CPP__ZENOH_SERVICE_HPP_
+#define RMW_ZENOH_CPP__ZENOH_SERVICE_HPP_
+
+#include <unordered_map>
+#include <utility>
+#include <string>
+#include <vector>
+
+extern "C"
+{
+  #include "zenoh/zenoh-ffi.h"
+}
+
+#include "rmw/rmw.h"
+#include "rmw_zenoh_cpp/TypeSupport.hpp"
+
+struct rmw_service_data_t
+{
+  /// STATIC MEMBERS ===========================================================
+  static void zn_request_sub_callback(const zn_sample * sample);
+
+  // Serialized ROS request messages
+  static std::unordered_map<std::string, std::vector<unsigned char> > zn_request_messages_;
+
+  /// TYPE SUPPORT =============================================================
+  const void * request_type_support_impl_;
+  const void * response_type_support_impl_;
+  const char * typesupport_identifier_;
+
+  rmw_zenoh_cpp::TypeSupport * request_type_support_;
+  rmw_zenoh_cpp::TypeSupport * response_type_support_;
+
+  /// ZENOH ====================================================================
+  ZNSession * zn_session_;
+  ZNQueryable * zn_queryable_;
+
+  // Request Sub
+  const char * zn_request_topic_key_;
+  ZNSubscriber * zn_request_subscriber_;
+
+  // Response Pub
+  const char * zn_response_topic_key_;
+  size_t zn_response_topic_id_;
+
+  /// ROS ======================================================================
+  const rmw_node_t * node_;
+};
+
+#endif // RMW_ZENOH_CPP__ZENOH_SERVICE_HPP_

rmw_zenoh_cpp/src/impl/type_support_common.cpp

@@ -99,34 +99,32 @@ MessageTypeSupport::MessageTypeSupport(const message_type_support_callbacks_t *
   set_members(members);
 }
 
+ServiceTypeSupport::ServiceTypeSupport()
+{
+}
+
+RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members)
+{
+  assert(members);
+
+  auto msg = static_cast<const message_type_support_callbacks_t *>(
+    members->request_members_->data);
+  // std::string name = _create_type_name(msg);  // + "Request_";
+  // this->setName(name.c_str());
 
+  set_members(msg);
+}
+
+ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members)
+{
+  assert(members);
 
-// ServiceTypeSupport::ServiceTypeSupport()
-// {
-// }
-//
-// RequestTypeSupport::RequestTypeSupport(const service_type_support_callbacks_t * members)
-// {
-//   assert(members);
-//
-//   auto msg = static_cast<const message_type_support_callbacks_t *>(
-//     members->request_members_->data);
-//   std::string name = _create_type_name(msg);  // + "Request_";
-//   this->setName(name.c_str());
-//
-//   set_members(msg);
-// }
-//
-// ResponseTypeSupport::ResponseTypeSupport(const service_type_support_callbacks_t * members)
-// {
-//   assert(members);
-//
-//   auto msg = static_cast<const message_type_support_callbacks_t *>(
-//     members->response_members_->data);
-//   std::string name = _create_type_name(msg);  // + "Response_";
-//   this->setName(name.c_str());
-//
-//   set_members(msg);
-// }
+  auto msg = static_cast<const message_type_support_callbacks_t *>(
+    members->response_members_->data);
+  // std::string name = _create_type_name(msg);  // + "Response_";
+  // this->setName(name.c_str());
+
+  set_members(msg);
+}
 
 }  // namespace rmw_zenoh_cpp

rmw_zenoh_cpp/src/impl/type_support_common.hpp

@@ -9,22 +9,22 @@
 #include "rmw_zenoh_cpp/TypeSupport.hpp"
 
 #include "rmw_zenoh_cpp/MessageTypeSupport.hpp"
-// #include "rmw_zenoh_cpp/ServiceTypeSupport.hpp"
+#include "rmw_zenoh_cpp/ServiceTypeSupport.hpp"
 
 #include "rmw_zenoh_cpp/identifier.hpp"
 
 #include "rosidl_typesupport_zenoh_c/identifier.h"
 #include "rosidl_typesupport_zenoh_cpp/identifier.hpp"
 #include "rosidl_typesupport_zenoh_cpp/message_type_support.h"
-// #include "rosidl_typesupport_zenoh_cpp/service_type_support.h"
+#include "rosidl_typesupport_zenoh_cpp/service_type_support.h"
 
 #define RMW_ZENOH_CPP_TYPESUPPORT_C rosidl_typesupport_zenoh_c__identifier
 #define RMW_ZENOH_CPP_TYPESUPPORT_CPP rosidl_typesupport_zenoh_cpp::typesupport_identifier
 
 using TypeSupport_cpp = rmw_zenoh_cpp::TypeSupport;
 using MessageTypeSupport_cpp = rmw_zenoh_cpp::MessageTypeSupport;
-// using RequestTypeSupport_cpp = rmw_zenoh_cpp::RequestTypeSupport;
-// using ResponseTypeSupport_cpp = rmw_zenoh_cpp::ResponseTypeSupport;
+using RequestTypeSupport_cpp = rmw_zenoh_cpp::RequestTypeSupport;
+using ResponseTypeSupport_cpp = rmw_zenoh_cpp::ResponseTypeSupport;
 
 // inline std::string
 // _create_type_name(