Temporary removes the on_new_[message]event on disable_callbacks()

Note: While we are temporary removes the on new message callback and all
 on new event callbacks from the underlying rmw layer to prevent them
 from being called. We can't guarantee that the currently executing
 on_new_[message]event callbacks are finished before we exit from the
 disable_callbacks().

Signed-off-by: Michael Orlov <[email protected]>

Author: MichaelOrlov

rclcpp/include/rclcpp/event_handler.hpp

@@ -324,15 +324,40 @@ class EventHandler : public EventHandlerBase
   }
 
   /// Disable the event callback from being called when execute(..) invoked
+  /**
+   * This will also temporarily remove the on_new_event_callback from the underlying rmw layer,
+   *  so that it is not called from the middleware while disabled.
+   */
   void disable() override
   {
+    {
+      // Temporary remove the on_new_event_callback_ to prevent it from being called
+      std::lock_guard<std::recursive_mutex> on_new_event_lock(on_new_event_callback_mutex_);
+      if (on_new_event_callback_) {
+        set_on_new_event_callback(nullptr, nullptr);
+      }
+    }
     std::lock_guard<std::mutex> event_callback_lock(event_callback_mutex_);
     disabled_.store(true);
   }
 
   /// Enable the event callback to be called when execute(..) invoked
+  /**
+   * This will also set back the on_new_event_callback to the underlying rmw layer, if it was
+   * previously removed with disable().
+   */
   void enable() override
   {
+    {
+      // Set callback again if it was previously removed in disable()
+      std::lock_guard<std::recursive_mutex> on_new_event_lock(on_new_event_callback_mutex_);
+      if (on_new_event_callback_) {
+        set_on_new_event_callback(
+          rclcpp::detail::cpp_callback_trampoline<
+            decltype(on_new_event_callback_), const void *, size_t>,
+          static_cast<const void *>(&on_new_event_callback_));
+      }
+    }
     std::lock_guard<std::mutex> event_callback_lock(event_callback_mutex_);
     disabled_.store(false);
   }

rclcpp/include/rclcpp/experimental/subscription_intra_process.hpp

@@ -154,13 +154,26 @@ class SubscriptionIntraProcess
     execute_impl<SubscribedType>(data);
   }
 
+  /// Disable callbacks from being called
+  /**
+    * This method will block, until any subscription's callbacks currently being executed are
+    * finished.
+    * This method is thread safe, and provides a safe way to atomically disable the callbacks.
+    */
   void disable_callbacks() override
   {
+    SubscriptionIntraProcessBase::disable_callbacks();
     any_callback_.disable();
   }
 
+  /// Enable the callbacks to be called
+  /**
+    * This method is thread safe, and provides a safe way to atomically enable the callbacks
+    * in a multithreaded environment.
+    */
   void enable_callbacks() override
   {
+    SubscriptionIntraProcessBase::enable_callbacks();
     any_callback_.enable();
   }
 

rclcpp/include/rclcpp/experimental/subscription_intra_process_base.hpp

@@ -88,14 +88,20 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   execute(const std::shared_ptr<void> & data) override = 0;
 
   /// Disable callbacks from being called
+  /**
+   * This function temporary disable on_new_message_callback to prevent it from being called.
+   */
   RCLCPP_PUBLIC
   virtual
-  void disable_callbacks() = 0;
+  void disable_callbacks();
 
   /// Enable the callbacks to be called
+  /**
+    * This function enable the on_new_message_callback if it was previously set.
+    */
   RCLCPP_PUBLIC
   virtual
-  void enable_callbacks() = 0;
+  void enable_callbacks();
 
   virtual
   bool
@@ -168,7 +174,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
         }
       };
 
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
     on_new_message_callback_ = new_callback;
 
     if (unread_count_ > 0) {
@@ -186,7 +192,7 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   void
   clear_on_ready_callback() override
   {
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
     on_new_message_callback_ = nullptr;
   }
 
@@ -198,8 +204,9 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   }
 
 protected:
-  std::recursive_mutex callback_mutex_;
+  std::recursive_mutex on_new_message_callback_mutex_;
   std::function<void(size_t)> on_new_message_callback_ {nullptr};
+  bool on_new_message_callback_disabled_{false};
   size_t unread_count_{0};
   rclcpp::GuardCondition gc_;
 
@@ -209,11 +216,13 @@ class SubscriptionIntraProcessBase : public rclcpp::Waitable
   void
   invoke_on_new_message()
   {
-    std::lock_guard<std::recursive_mutex> lock(this->callback_mutex_);
-    if (this->on_new_message_callback_) {
-      this->on_new_message_callback_(1);
-    } else {
-      this->unread_count_++;
+    std::lock_guard<std::recursive_mutex> lock(this->on_new_message_callback_mutex_);
+    if (!on_new_message_callback_disabled_) {
+      if (this->on_new_message_callback_) {
+        this->on_new_message_callback_(1);
+      } else {
+        this->unread_count_++;
+      }
     }
   }
 

rclcpp/include/rclcpp/generic_subscription.hpp

@@ -111,9 +111,23 @@ class GenericSubscription : public rclcpp::SubscriptionBase
   RCLCPP_PUBLIC
   std::shared_ptr<rclcpp::SerializedMessage> create_serialized_message() override;
 
+  /// Disable callbacks from being called
+  /**
+    * This method will block, until any subscription's callbacks provided during construction
+    * currently being executed are finished.
+    * \note This method also temporary removes the on new message callback and all
+    * on new event callbacks from the rmw layer to prevent them from being called. However, this
+    * method will not block and wait until the currently executing on_new_[message]event callbacks
+    * are finished.
+    */
   RCLCPP_PUBLIC
   void disable_callbacks() override;
 
+  /// Enable the callbacks to be called
+  /**
+    * This method is thread safe, and provides a safe way to atomically enable the callbacks
+    * in a multithreaded environment.
+    */
   RCLCPP_PUBLIC
   void enable_callbacks() override;
 

rclcpp/include/rclcpp/subscription.hpp

@@ -279,10 +279,19 @@ class Subscription : public SubscriptionBase
     return message_memory_strategy_->borrow_serialized_message();
   }
 
-
+  /// Disable callbacks from being called
+  /**
+    * This method will block, until any subscription's callbacks provided during construction
+    * currently being executed are finished.
+    * \note This method also temporary removes the on new message callback and all
+    * on new event callbacks from the rmw layer to prevent them from being called. However, this
+    * method will not block and wait until the currently executing on_new_[message]event callbacks
+    * are finished.
+    */
   void
   disable_callbacks() override
   {
+    SubscriptionBase::disable_callbacks();
     any_callback_.disable();
     if (subscription_intra_process_) {
       subscription_intra_process_->disable_callbacks();
@@ -294,9 +303,15 @@ class Subscription : public SubscriptionBase
     }
   }
 
+  /// Enable the callbacks to be called
+  /**
+    * This method is thread safe, and provides a safe way to atomically enable the callbacks
+    * in a multithreaded environment.
+    */
   void
   enable_callbacks() override
   {
+    SubscriptionBase::enable_callbacks();
     any_callback_.enable();
     if (subscription_intra_process_) {
       subscription_intra_process_->enable_callbacks();

rclcpp/include/rclcpp/subscription_base.hpp

@@ -213,14 +213,21 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   create_serialized_message() = 0;
 
   /// Disable callbacks from being called
+  /**
+   * This function temporary removes the on_new_message_callback to prevent it from being called.
+   */
   RCLCPP_PUBLIC
   virtual
-  void disable_callbacks() = 0;
+  void disable_callbacks();
 
   /// Enable the callbacks to be called
+  /**
+    * This function sets back the on_new_message_callback if it was previously removed in
+    * disable_callbacks().
+    */
   RCLCPP_PUBLIC
   virtual
-  void enable_callbacks() = 0;
+  void enable_callbacks();
 
   /// Check if we need to handle the message, and execute the callback if we do.
   /**
@@ -393,7 +400,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
         }
       };
 
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
 
     // Set it temporarily to the new callback, while we replace the old one.
     // This two-step setting, prevents a gap where the old std::function has
@@ -416,7 +423,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
   void
   clear_on_new_message_callback()
   {
-    std::lock_guard<std::recursive_mutex> lock(callback_mutex_);
+    std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
 
     if (on_new_message_callback_) {
       set_on_new_message_callback(nullptr, nullptr);
@@ -656,7 +663,7 @@ class SubscriptionBase : public std::enable_shared_from_this<SubscriptionBase>
 
   std::shared_ptr<rcl_node_t> node_handle_;
 
-  std::recursive_mutex callback_mutex_;
+  std::recursive_mutex on_new_message_callback_mutex_;
   // It is important to declare on_new_message_callback_ before
   // subscription_handle_, so on destruction the subscription is
   // destroyed first. Otherwise, the rmw subscription callback

rclcpp/src/rclcpp/generic_subscription.cpp

@@ -40,6 +40,7 @@ GenericSubscription::create_serialized_message()
 void
 GenericSubscription::disable_callbacks()
 {
+  SubscriptionBase::disable_callbacks();
   any_callback_.disable();
   for (const auto & [_, event_ptr] : event_handlers_) {
     if (event_ptr) {
@@ -51,6 +52,7 @@ GenericSubscription::disable_callbacks()
 void
 GenericSubscription::enable_callbacks()
 {
+  SubscriptionBase::enable_callbacks();
   any_callback_.enable();
   for (const auto & [_, event_ptr] : event_handlers_) {
     if (event_ptr) {

rclcpp/src/rclcpp/subscription_base.cpp

@@ -577,3 +577,26 @@ SubscriptionBase::take_dynamic_message(
   throw std::runtime_error("Unimplemented");
   return false;
 }
+
+void
+SubscriptionBase::disable_callbacks()
+{
+  // Temporary remove the on_new_message_callback_ to prevent it from being called
+  std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+  if (on_new_message_callback_) {
+    set_on_new_message_callback(nullptr, nullptr);
+  }
+}
+
+void
+SubscriptionBase::enable_callbacks()
+{
+  // Set callback again if it was previously removed in disable_callbacks()
+  std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+  if (on_new_message_callback_) {
+    set_on_new_message_callback(
+      rclcpp::detail::cpp_callback_trampoline<
+        decltype(on_new_message_callback_), const void *, size_t>,
+      static_cast<const void *>(&on_new_message_callback_));
+  }
+}

rclcpp/src/rclcpp/subscription_intra_process_base.cpp

@@ -40,3 +40,17 @@ SubscriptionIntraProcessBase::is_durability_transient_local() const
 {
   return qos_profile_.durability() == rclcpp::DurabilityPolicy::TransientLocal;
 }
+
+void
+SubscriptionIntraProcessBase::disable_callbacks()
+{
+  std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+  on_new_message_callback_disabled_ = true;
+}
+
+void
+SubscriptionIntraProcessBase::enable_callbacks()
+{
+  std::lock_guard<std::recursive_mutex> lock(on_new_message_callback_mutex_);
+  on_new_message_callback_disabled_ = false;
+}