More overall improvements to the code (#98)

* Move more implementation into rmw_subscription_data_t.

This moves more of the implementation out of rmw_zenoh.cpp
and into the class handling the subscription data.

Signed-off-by: Chris Lalancette <[email protected]>

* Move more of the implementation into rmw_service_data_t.

This just allows us to hide more of the implementation and
remove it from rmw_zenoh.cpp.  It also fixes some of the
locking, which wasn't correct previously.

Signed-off-by: Chris Lalancette <[email protected]>

* Move more of the implementation into rmw_client_data_t.

This allows us to encapsulate more of the client data in
the right place, and also allows us to fix the locking.

Signed-off-by: Chris Lalancette <[email protected]>

* Make sure to reset the trigger.

That is, once we have discovered what the guard condition
status is, we should immediately reset the trigger condition
and wait for the next one.  Otherwise we risk triggering
over and over again.

Signed-off-by: Chris Lalancette <[email protected]>

---------

Signed-off-by: Chris Lalancette <[email protected]>

rmw_zenoh_cpp/src/detail/guard_condition.cpp

@@ -52,15 +52,14 @@ void GuardCondition::detach_condition()
 }
 
 ///==============================================================================
-bool GuardCondition::has_triggered() const
+bool GuardCondition::get_and_reset_trigger()
 {
   std::lock_guard<std::mutex> lock(internal_mutex_);
-  return has_triggered_;
-}
+  bool ret = has_triggered_;
 
-///==============================================================================
-void GuardCondition::reset_trigger()
-{
-  std::lock_guard<std::mutex> lock(internal_mutex_);
+  // There is no data associated with the guard condition, so as soon as the callers asks about the
+  // state, we can immediately reset and get ready for the next trigger.
   has_triggered_ = false;
+
+  return ret;
 }

rmw_zenoh_cpp/src/detail/guard_condition.hpp

@@ -33,10 +33,7 @@ class GuardCondition final
 
   void detach_condition();
 
-  bool has_triggered() const;
-
-  // Resets has_triggered_ to false.
-  void reset_trigger();
+  bool get_and_reset_trigger();
 
 private:
   mutable std::mutex internal_mutex_;

rmw_zenoh_cpp/src/detail/rmw_data_types.cpp

@@ -31,6 +31,196 @@ saved_msg_data::saved_msg_data(zc_owned_payload_t p, uint64_t recv_ts, const uin
   memcpy(publisher_gid, pub_gid, 16);
 }
 
+void rmw_subscription_data_t::attach_condition(std::condition_variable * condition_variable)
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = condition_variable;
+}
+
+void rmw_subscription_data_t::notify()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  if (condition_ != nullptr) {
+    condition_->notify_one();
+  }
+}
+
+void rmw_subscription_data_t::detach_condition()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = nullptr;
+}
+
+bool rmw_subscription_data_t::message_queue_is_empty() const
+{
+  std::lock_guard<std::mutex> lock(message_queue_mutex_);
+  return message_queue_.empty();
+}
+
+std::unique_ptr<saved_msg_data> rmw_subscription_data_t::pop_next_message()
+{
+  std::lock_guard<std::mutex> lock(message_queue_mutex_);
+
+  if (message_queue_.empty()) {
+    // This tells rcl that the check for a new message was done, but no messages have come in yet.
+    return nullptr;
+  }
+
+  std::unique_ptr<saved_msg_data> msg_data = std::move(message_queue_.front());
+  message_queue_.pop_front();
+
+  return msg_data;
+}
+
+void rmw_subscription_data_t::add_new_message(
+  std::unique_ptr<saved_msg_data> msg, const std::string & topic_name)
+{
+  std::lock_guard<std::mutex> lock(message_queue_mutex_);
+
+  if (message_queue_.size() >= queue_depth) {
+    // Log warning if message is discarded due to hitting the queue depth
+    RCUTILS_LOG_WARN_NAMED(
+      "rmw_zenoh_cpp",
+      "Message queue depth of %ld reached, discarding oldest message "
+      "for subscription for %s",
+      queue_depth,
+      topic_name.c_str());
+
+    std::unique_ptr<saved_msg_data> old = std::move(message_queue_.front());
+    z_drop(z_move(old->payload));
+    message_queue_.pop_front();
+  }
+
+  message_queue_.emplace_back(std::move(msg));
+
+  // Since we added new data, trigger the guard condition if it is available
+  notify();
+}
+
+bool rmw_service_data_t::query_queue_is_empty() const
+{
+  std::lock_guard<std::mutex> lock(query_queue_mutex_);
+  return query_queue_.empty();
+}
+
+void rmw_service_data_t::attach_condition(std::condition_variable * condition_variable)
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = condition_variable;
+}
+
+void rmw_service_data_t::detach_condition()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = nullptr;
+}
+
+std::unique_ptr<ZenohQuery> rmw_service_data_t::pop_next_query()
+{
+  std::lock_guard<std::mutex> lock(query_queue_mutex_);
+  if (query_queue_.empty()) {
+    return nullptr;
+  }
+
+  std::unique_ptr<ZenohQuery> query = std::move(query_queue_.front());
+  query_queue_.pop_front();
+
+  return query;
+}
+
+void rmw_service_data_t::notify()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  if (condition_ != nullptr) {
+    condition_->notify_one();
+  }
+}
+
+void rmw_service_data_t::add_new_query(std::unique_ptr<ZenohQuery> query)
+{
+  std::lock_guard<std::mutex> lock(query_queue_mutex_);
+  query_queue_.emplace_back(std::move(query));
+
+  // Since we added new data, trigger the guard condition if it is available
+  notify();
+}
+
+bool rmw_service_data_t::add_to_query_map(
+  int64_t sequence_number, std::unique_ptr<ZenohQuery> query)
+{
+  std::lock_guard<std::mutex> lock(sequence_to_query_map_mutex_);
+  if (sequence_to_query_map_.find(sequence_number) != sequence_to_query_map_.end()) {
+    return false;
+  }
+  sequence_to_query_map_.emplace(
+    std::pair(sequence_number, std::move(query)));
+
+  return true;
+}
+
+std::unique_ptr<ZenohQuery> rmw_service_data_t::take_from_query_map(int64_t sequence_number)
+{
+  std::lock_guard<std::mutex> lock(sequence_to_query_map_mutex_);
+  auto query_it = sequence_to_query_map_.find(sequence_number);
+  if (query_it == sequence_to_query_map_.end()) {
+    return nullptr;
+  }
+
+  std::unique_ptr<ZenohQuery> query = std::move(query_it->second);
+  sequence_to_query_map_.erase(query_it);
+
+  return query;
+}
+
+void rmw_client_data_t::notify()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  if (condition_ != nullptr) {
+    condition_->notify_one();
+  }
+}
+
+void rmw_client_data_t::add_new_reply(std::unique_ptr<ZenohReply> reply)
+{
+  std::lock_guard<std::mutex> lock(reply_queue_mutex_);
+  reply_queue_.emplace_back(std::move(reply));
+
+  notify();
+}
+
+bool rmw_client_data_t::reply_queue_is_empty() const
+{
+  std::lock_guard<std::mutex> lock(reply_queue_mutex_);
+
+  return reply_queue_.empty();
+}
+
+void rmw_client_data_t::attach_condition(std::condition_variable * condition_variable)
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = condition_variable;
+}
+
+void rmw_client_data_t::detach_condition()
+{
+  std::lock_guard<std::mutex> lock(condition_mutex_);
+  condition_ = nullptr;
+}
+
+std::unique_ptr<ZenohReply> rmw_client_data_t::pop_next_reply()
+{
+  std::lock_guard<std::mutex> lock(reply_queue_mutex_);
+
+  if (reply_queue_.empty()) {
+    return nullptr;
+  }
+
+  std::unique_ptr<ZenohReply> latest_reply = std::move(reply_queue_.front());
+  reply_queue_.pop_front();
+
+  return latest_reply;
+}
+
 //==============================================================================
 void sub_data_handler(
   const z_sample_t * sample,
@@ -53,34 +243,10 @@ void sub_data_handler(
     return;
   }
 
-  {
-    std::lock_guard<std::mutex> lock(sub_data->message_queue_mutex);
-
-    if (sub_data->message_queue.size() >= sub_data->queue_depth) {
-      // Log warning if message is discarded due to hitting the queue depth
-      RCUTILS_LOG_WARN_NAMED(
-        "rmw_zenoh_cpp",
-        "Message queue depth of %ld reached, discarding oldest message "
-        "for subscription for %s",
-        sub_data->queue_depth,
-        z_loan(keystr));
-
-      std::unique_ptr<saved_msg_data> old = std::move(sub_data->message_queue.front());
-      z_drop(&old->payload);
-      sub_data->message_queue.pop_front();
-    }
-
-    sub_data->message_queue.emplace_back(
-      std::make_unique<saved_msg_data>(
-        zc_sample_payload_rcinc(sample),
-        sample->timestamp.time, sample->timestamp.id.id));
-
-    // Since we added new data, trigger the guard condition if it is available
-    std::lock_guard<std::mutex> internal_lock(sub_data->internal_mutex);
-    if (sub_data->condition != nullptr) {
-      sub_data->condition->notify_one();
-    }
-  }
+  sub_data->add_new_message(
+    std::make_unique<saved_msg_data>(
+      zc_sample_payload_rcinc(sample),
+      sample->timestamp.time, sample->timestamp.id.id), z_loan(keystr));
 }
 
 ZenohQuery::ZenohQuery(const z_query_t * query)
@@ -118,18 +284,7 @@ void service_data_handler(const z_query_t * query, void * data)
     return;
   }
 
-  // Get the query parameters and payload
-  {
-    std::lock_guard<std::mutex> lock(service_data->query_queue_mutex);
-    service_data->query_queue.emplace_back(std::make_unique<ZenohQuery>(query));
-  }
-  {
-    // Since we added new data, trigger the guard condition if it is available
-    std::lock_guard<std::mutex> internal_lock(service_data->internal_mutex);
-    if (service_data->condition != nullptr) {
-      service_data->condition->notify_one();
-    }
-  }
+  service_data->add_new_query(std::make_unique<ZenohQuery>(query));
 }
 
 ZenohReply::ZenohReply(const z_owned_reply_t * reply)
@@ -182,16 +337,8 @@ void client_data_handler(z_owned_reply_t * reply, void * data)
     );
     return;
   }
-  {
-    std::lock_guard<std::mutex> msg_lock(client_data->replies_mutex);
-    // Take ownership of the reply.
-    client_data->replies.emplace_back(std::make_unique<ZenohReply>(reply));
-    *reply = z_reply_null();
-  }
-  {
-    std::lock_guard<std::mutex> internal_lock(client_data->internal_mutex);
-    if (client_data->condition != nullptr) {
-      client_data->condition->notify_one();
-    }
-  }
+
+  client_data->add_new_reply(std::make_unique<ZenohReply>(reply));
+  // Since we took ownership of the reply, null it out here
+  *reply = z_reply_null();
 }