Fix Groot2 publisher thread safety and hook handling

CMakeLists.txt

@@ -185,6 +185,7 @@ list(APPEND BT_SOURCE
     src/controls/try_catch_node.cpp
     src/controls/while_do_else_node.cpp
 
+    src/loggers/abstract_logger.cpp
     src/loggers/bt_cout_logger.cpp
     src/loggers/bt_file_logger_v2.cpp
     src/loggers/bt_minitrace_logger.cpp

include/behaviortree_cpp/loggers/abstract_logger.h

@@ -4,6 +4,8 @@
 #include "behaviortree_cpp/behavior_tree.h"
 #include "behaviortree_cpp/bt_factory.h"
 
+#include <memory>
+
 namespace BT
 {
 enum class TimestampType
@@ -18,7 +20,7 @@ class StatusChangeLogger
   /// Construct and immediately subscribe to status changes.
   StatusChangeLogger(TreeNode* root_node);
 
-  virtual ~StatusChangeLogger() = default;
+  virtual ~StatusChangeLogger();
 
   StatusChangeLogger(const StatusChangeLogger& other) = delete;
   StatusChangeLogger& operator=(const StatusChangeLogger& other) = delete;
@@ -31,87 +33,38 @@ class StatusChangeLogger
 
   virtual void flush() = 0;
 
-  void setEnabled(bool enabled)
-  {
-    enabled_ = enabled;
-  }
+  void setEnabled(bool enabled);
 
-  void setTimestampType(TimestampType type)
-  {
-    type_ = type;
-  }
+  void setTimestampType(TimestampType type);
 
-  bool enabled() const
-  {
-    return enabled_;
-  }
+  bool enabled() const;
 
   // false by default.
-  bool showsTransitionToIdle() const
-  {
-    return show_transition_to_idle_;
-  }
+  bool showsTransitionToIdle() const;
 
-  void enableTransitionToIdle(bool enable)
-  {
-    show_transition_to_idle_ = enable;
-  }
+  void enableTransitionToIdle(bool enable);
 
 protected:
   /// Default constructor for deferred subscription. Call subscribeToTreeChanges() when ready.
-  StatusChangeLogger() = default;
+  StatusChangeLogger();
 
   /// Subscribe to status changes. Call at end of constructor for deferred subscription.
   void subscribeToTreeChanges(TreeNode* root_node);
 
+  /// Stop new callbacks and wait until callbacks already in progress have completed.
+  /// Derived destructors must call this before destroying state used by callback().
+  void unsubscribeFromTreeChanges();
+
 private:
-  bool enabled_ = true;
-  bool show_transition_to_idle_ = true;
-  std::vector<TreeNode::StatusChangeSubscriber> subscribers_;
-  TimestampType type_ = TimestampType::absolute;
-  BT::TimePoint first_timestamp_ = {};
-  std::mutex callback_mutex_;
+  /// Forward a status transition to callback(), unless disabled.
+  void handleStatusChange(TimePoint timestamp, const TreeNode& node, NodeStatus prev,
+                          NodeStatus status);
+
+  // All state lives behind this pointer, so that changing it does not alter the
+  // layout that derived classes (including user-defined loggers) compile against.
+  struct PImpl;
+  std::unique_ptr<PImpl> _p;
 };
-
-//--------------------------------------------
-
-inline StatusChangeLogger::StatusChangeLogger(TreeNode* root_node)
-{
-  subscribeToTreeChanges(root_node);
-}
-
-inline void StatusChangeLogger::subscribeToTreeChanges(TreeNode* root_node)
-{
-  first_timestamp_ = std::chrono::high_resolution_clock::now();
-
-  auto subscribeCallback = [this](TimePoint timestamp, const TreeNode& node,
-                                  NodeStatus prev, NodeStatus status) {
-    // Copy state under lock, then release before calling user code
-    // This prevents recursive mutex locking when multiple nodes change status
-    bool should_callback = false;
-    Duration adjusted_timestamp;
-    {
-      std::unique_lock lk(callback_mutex_);
-      if(enabled_ && (status != NodeStatus::IDLE || show_transition_to_idle_))
-      {
-        should_callback = true;
-        adjusted_timestamp = (type_ == TimestampType::absolute) ?
-                                 timestamp.time_since_epoch() :
-                                 (timestamp - first_timestamp_);
-      }
-    }
-    if(should_callback)
-    {
-      this->callback(adjusted_timestamp, node, prev, status);
-    }
-  };
-
-  auto visitor = [this, subscribeCallback](TreeNode* node) {
-    subscribers_.push_back(node->subscribeToStatusChange(std::move(subscribeCallback)));
-  };
-
-  applyRecursiveVisitor(root_node, visitor);
-}
 }  // namespace BT
 
 #endif  // ABSTRACT_LOGGER_H

include/behaviortree_cpp/utils/callback_gate.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include <condition_variable>
+#include <memory>
+#include <mutex>
+
+namespace BT::details
+{
+
+/**
+ * @brief Synchronization gate used to coordinate callbacks with object teardown.
+ *
+ * Callbacks enter the gate with tryStart() and must signal completion when done
+ * (use Guard for RAII). During teardown, call close() to reject new callbacks
+ * and closeAndDrain() to block until the callbacks still running have completed.
+ */
+class CallbackGate
+{
+public:
+  using Ptr = std::shared_ptr<CallbackGate>;
+
+  /// RAII helper that signals the completion of a callback entered with tryStart()
+  class Guard
+  {
+  public:
+    explicit Guard(Ptr gate) : gate_(std::move(gate))
+    {}
+
+    Guard(const Guard&) = delete;
+    Guard& operator=(const Guard&) = delete;
+    Guard(Guard&&) = delete;
+    Guard& operator=(Guard&&) = delete;
+
+    ~Guard()
+    {
+      gate_->completed();
+    }
+
+  private:
+    Ptr gate_;
+  };
+
+  /// Returns false if the gate has been closed and the callback must not run.
+  bool tryStart()
+  {
+    const std::lock_guard lk(mutex_);
+    if(!accepting_callbacks_)
+    {
+      return false;
+    }
+    running_callbacks_++;
+    return true;
+  }
+
+  /// Reject new callbacks, without waiting for the running ones.
+  void close()
+  {
+    const std::lock_guard lk(mutex_);
+    accepting_callbacks_ = false;
+  }
+
+  /// Reject new callbacks and wait until callbacks already running have completed.
+  void closeAndDrain()
+  {
+    std::unique_lock lk(mutex_);
+    accepting_callbacks_ = false;
+    condition_variable_.wait(lk, [this] { return running_callbacks_ == 0; });
+  }
+
+private:
+  void completed()
+  {
+    const std::lock_guard lk(mutex_);
+    running_callbacks_--;
+    if(running_callbacks_ == 0 && !accepting_callbacks_)
+    {
+      condition_variable_.notify_all();
+    }
+  }
+
+  std::mutex mutex_;
+  std::condition_variable condition_variable_;
+  bool accepting_callbacks_ = true;
+  size_t running_callbacks_ = 0;
+};
+
+}  // namespace BT::details

src/loggers/abstract_logger.cpp

@@ -0,0 +1,117 @@
+#include "behaviortree_cpp/loggers/abstract_logger.h"
+
+#include "behaviortree_cpp/utils/callback_gate.h"
+
+#include <mutex>
+
+namespace BT
+{
+
+struct StatusChangeLogger::PImpl
+{
+  bool enabled = true;
+  bool show_transition_to_idle = true;
+  std::vector<TreeNode::StatusChangeSubscriber> subscribers;
+  TimestampType type = TimestampType::absolute;
+  BT::TimePoint first_timestamp = {};
+  std::mutex callback_mutex;
+  details::CallbackGate::Ptr callback_gate = std::make_shared<details::CallbackGate>();
+};
+
+StatusChangeLogger::StatusChangeLogger() : _p(std::make_unique<PImpl>())
+{}
+
+StatusChangeLogger::StatusChangeLogger(TreeNode* root_node) : StatusChangeLogger()
+{
+  subscribeToTreeChanges(root_node);
+}
+
+StatusChangeLogger::~StatusChangeLogger()
+{
+  unsubscribeFromTreeChanges();
+}
+
+void StatusChangeLogger::setEnabled(bool enabled)
+{
+  const std::lock_guard lk(_p->callback_mutex);
+  _p->enabled = enabled;
+}
+
+void StatusChangeLogger::setTimestampType(TimestampType type)
+{
+  const std::lock_guard lk(_p->callback_mutex);
+  _p->type = type;
+}
+
+bool StatusChangeLogger::enabled() const
+{
+  const std::lock_guard lk(_p->callback_mutex);
+  return _p->enabled;
+}
+
+bool StatusChangeLogger::showsTransitionToIdle() const
+{
+  const std::lock_guard lk(_p->callback_mutex);
+  return _p->show_transition_to_idle;
+}
+
+void StatusChangeLogger::enableTransitionToIdle(bool enable)
+{
+  const std::lock_guard lk(_p->callback_mutex);
+  _p->show_transition_to_idle = enable;
+}
+
+void StatusChangeLogger::subscribeToTreeChanges(TreeNode* root_node)
+{
+  _p->first_timestamp = std::chrono::high_resolution_clock::now();
+
+  auto subscribeCallback =
+      [this, gate = _p->callback_gate](TimePoint timestamp, const TreeNode& node,
+                                       NodeStatus prev, NodeStatus status) {
+        if(!gate->tryStart())
+        {
+          return;
+        }
+        const details::CallbackGate::Guard callback_guard(gate);
+        handleStatusChange(timestamp, node, prev, status);
+      };
+
+  auto visitor = [this, subscribeCallback](TreeNode* node) {
+    _p->subscribers.push_back(
+        node->subscribeToStatusChange(std::move(subscribeCallback)));
+  };
+
+  applyRecursiveVisitor(root_node, visitor);
+}
+
+void StatusChangeLogger::handleStatusChange(TimePoint timestamp, const TreeNode& node,
+                                            NodeStatus prev, NodeStatus status)
+{
+  // Copy state under lock, then release before calling user code
+  // This prevents recursive mutex locking when multiple nodes change status
+  bool should_callback = false;
+  Duration adjusted_timestamp;
+  {
+    std::unique_lock lk(_p->callback_mutex);
+    if(_p->enabled && (status != NodeStatus::IDLE || _p->show_transition_to_idle))
+    {
+      should_callback = true;
+      adjusted_timestamp = (_p->type == TimestampType::absolute) ?
+                               timestamp.time_since_epoch() :
+                               (timestamp - _p->first_timestamp);
+    }
+  }
+
+  if(should_callback)
+  {
+    this->callback(adjusted_timestamp, node, prev, status);
+  }
+}
+
+void StatusChangeLogger::unsubscribeFromTreeChanges()
+{
+  _p->callback_gate->closeAndDrain();
+  _p->subscribers.clear();
+}
+
+}  // namespace BT

src/loggers/bt_cout_logger.cpp

@@ -5,7 +5,11 @@ namespace BT
 
 StdCoutLogger::StdCoutLogger(const BT::Tree& tree) : StatusChangeLogger(tree.rootNode())
 {}
-StdCoutLogger::~StdCoutLogger() = default;
+StdCoutLogger::~StdCoutLogger()
+{
+  // Stop status callbacks while the derived object is still intact.
+  unsubscribeFromTreeChanges();
+}
 
 void StdCoutLogger::callback(Duration timestamp, const TreeNode& node,
                              NodeStatus prev_status, NodeStatus status)

src/loggers/bt_file_logger_v2.cpp

@@ -94,6 +94,9 @@ FileLogger2::FileLogger2(const BT::Tree& tree, std::filesystem::path const& file
 
 FileLogger2::~FileLogger2()
 {
+  // Stop status callbacks before tearing down the state they use.
+  unsubscribeFromTreeChanges();
+
   _p->loop = false;
   _p->queue_cv.notify_one();
   _p->writer_thread.join();

src/loggers/bt_minitrace_logger.cpp

@@ -17,6 +17,9 @@ MinitraceLogger::MinitraceLogger(const Tree& tree, const char* filename_json)
 
 MinitraceLogger::~MinitraceLogger()
 {
+  // Stop status callbacks before shutting down minitrace.
+  unsubscribeFromTreeChanges();
+
   mtr_flush();
   mtr_shutdown();
 }

src/loggers/bt_observer.cpp

@@ -45,7 +45,10 @@ TreeObserver::TreeObserver(const BT::Tree& tree) : StatusChangeLogger(tree.rootN
 }
 
 TreeObserver::~TreeObserver()
-{}
+{
+  // Stop status callbacks before the statistics map is destroyed.
+  unsubscribeFromTreeChanges();
+}
 
 void TreeObserver::callback(Duration timestamp, const TreeNode& node,
                             NodeStatus /*prev_status*/, NodeStatus status)

src/loggers/bt_sqlite_logger.cpp

@@ -140,6 +140,9 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
 
 SqliteLogger::~SqliteLogger()
 {
+  // Stop status callbacks before tearing down the state they use.
+  unsubscribeFromTreeChanges();
+
   try
   {
     loop_ = false;