wip: updating waitables to ensure things stay ready when needed, debugging code still there

Signed-off-by: William Woodall <[email protected]>

Author: wjwwood

rclcpp/include/rclcpp/event_handler.hpp

@@ -106,6 +106,12 @@ class EventHandlerBase : public Waitable
     Event,
   };
 
+  // TODO(wjwwood): is this ok? do events continue to stay ready until they are
+  // taken/checked?
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   RCLCPP_PUBLIC
   virtual ~EventHandlerBase();
 

rclcpp/include/rclcpp/executors/executor_notify_waitable.hpp

@@ -50,10 +50,13 @@ class ExecutorNotifyWaitable : public rclcpp::Waitable
   RCLCPP_PUBLIC
   ExecutorNotifyWaitable(ExecutorNotifyWaitable & other);
 
-
   RCLCPP_PUBLIC
   ExecutorNotifyWaitable & operator=(ExecutorNotifyWaitable & other);
 
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   /// Add conditions to the wait set
   /**
    * \param[inout] wait_set structure that conditions will be added to

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -101,6 +101,28 @@ class SubscriptionIntraProcess
 
   virtual ~SubscriptionIntraProcess() = default;
 
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
+  RCLCPP_PUBLIC
+  void
+  add_to_wait_set(rcl_wait_set_t * wait_set) override
+  {
+    // This block is necessary when the guard condition wakes the wait set, but
+    // the intra process waitable was not handled before the wait set is waited
+    // on again.
+    // Basically we're keeping the guard condition triggered so long as there is
+    // data in the buffer.
+    if (this->buffer_->has_data()) {
+      // If there is data still to be processed, indicate to the
+      // executor or waitset by triggering the guard condition.
+      this->trigger_guard_condition();
+    }
+    // Let the parent classes handle the rest of the work:
+    return SubscriptionIntraProcessBufferT::add_to_wait_set(wait_set);
+  }
+
   std::shared_ptr<void>
   take_data() override
   {

rclcpp/include/rclcpp/waitable.hpp

@@ -35,6 +35,11 @@ class Waitable
   RCLCPP_PUBLIC
   virtual ~Waitable() = default;
 
+  RCLCPP_PUBLIC
+  virtual
+  void
+  dummy() = 0;
+
   /// Get the number of ready subscriptions
   /**
    * Returns a value of 0 by default.

rclcpp/src/rclcpp/event_handler.cpp

@@ -15,6 +15,7 @@
 #include <stdexcept>
 #include <string>
 
+#include "rcl/error_handling.h"
 #include "rcl/event.h"
 
 #include "rclcpp/event_handler.hpp"

rclcpp/src/rclcpp/executors/executor_notify_waitable.cpp

@@ -48,6 +48,11 @@ void
 ExecutorNotifyWaitable::add_to_wait_set(rcl_wait_set_t * wait_set)
 {
   std::lock_guard<std::mutex> lock(guard_condition_mutex_);
+
+  // Note: no guard conditions need to be re-triggered, since the guard
+  // conditions in this class are not tracking a stateful condition, but instead
+  // only serve to interrupt the wait set when new information is available to
+  // consider.
   for (auto weak_guard_condition : this->notify_guard_conditions_) {
     auto guard_condition = weak_guard_condition.lock();
     if (!guard_condition) {continue;}

rclcpp/test/rclcpp/executors/test_executors.cpp

@@ -336,6 +336,12 @@ class TestWaitable : public rclcpp::Waitable
 public:
   TestWaitable() = default;
 
+  // TODO(wjwwood): is this ok? do events continue to stay ready until they are
+  // taken/checked?
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   void
   add_to_wait_set(rcl_wait_set_t * wait_set) override
   {

rclcpp/test/rclcpp/executors/test_static_executor_entities_collector.cpp

@@ -230,6 +230,12 @@ TEST_F(TestStaticExecutorEntitiesCollector, add_remove_node_out_of_scope) {
 class TestWaitable : public rclcpp::Waitable
 {
 public:
+  // TODO(wjwwood): is this ok? do events continue to stay ready until they are
+  // taken/checked?
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   void add_to_wait_set(rcl_wait_set_t *) override {}
 
   bool is_ready(rcl_wait_set_t *) override {return true;}

rclcpp/test/rclcpp/node_interfaces/test_node_waitables.cpp

@@ -28,6 +28,12 @@
 class TestWaitable : public rclcpp::Waitable
 {
 public:
+  // TODO(wjwwood): is this ok? do events continue to stay ready until they are
+  // taken/checked?
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   void add_to_wait_set(rcl_wait_set_t *) override {}
   bool is_ready(rcl_wait_set_t *) override {return false;}
 

rclcpp/test/rclcpp/strategies/test_allocator_memory_strategy.cpp

@@ -39,6 +39,11 @@ static bool test_waitable_result = false;
 class TestWaitable : public rclcpp::Waitable
 {
 public:
+  // TODO(wjwwood): is this ok? double check that this test doesn't need more
+  RCLCPP_PUBLIC
+  void
+  dummy() override {};
+
   void add_to_wait_set(rcl_wait_set_t *) override
   {
     if (!test_waitable_result) {