Fix Blackboard thread-safety bugs and XML null pointer dereference

CMakeLists.txt

@@ -180,6 +180,7 @@ list(APPEND BT_SOURCE
     src/controls/sequence_node.cpp
     src/controls/sequence_with_memory_node.cpp
     src/controls/switch_node.cpp
+    src/controls/try_catch_node.cpp
     src/controls/while_do_else_node.cpp
 
     src/loggers/bt_cout_logger.cpp

include/behaviortree_cpp/behavior_tree.h

@@ -33,6 +33,7 @@
 #include "behaviortree_cpp/controls/sequence_node.h"
 #include "behaviortree_cpp/controls/sequence_with_memory_node.h"
 #include "behaviortree_cpp/controls/switch_node.h"
+#include "behaviortree_cpp/controls/try_catch_node.h"
 #include "behaviortree_cpp/controls/while_do_else_node.h"
 #include "behaviortree_cpp/decorators/delay_node.h"
 #include "behaviortree_cpp/decorators/force_failure_node.h"

include/behaviortree_cpp/blackboard.h

@@ -9,6 +9,7 @@
 
 #include <memory>
 #include <mutex>
+#include <shared_mutex>
 #include <string>
 #include <unordered_map>
 
@@ -127,9 +128,6 @@ class Blackboard
   [[deprecated("This command is unsafe. Consider using Backup/Restore instead")]] void
   clear();
 
-  [[deprecated("Use getAnyLocked to access safely an Entry")]] std::recursive_mutex&
-  entryMutex() const;
-
   void createEntry(const std::string& key, const TypeInfo& info);
 
   /**
@@ -179,8 +177,7 @@ class Blackboard
   [[nodiscard]] Expected<T> tryCastWithPolymorphicFallback(const Any* any) const;
 
 private:
-  mutable std::mutex storage_mutex_;
-  mutable std::recursive_mutex entry_mutex_;
+  mutable std::shared_mutex storage_mutex_;
   std::unordered_map<std::string, std::shared_ptr<Entry>> storage_;
   std::weak_ptr<Blackboard> parent_bb_;
   std::unordered_map<std::string, std::string> internal_to_external_;
@@ -279,7 +276,7 @@ inline void Blackboard::set(const std::string& key, const T& value)
     rootBlackboard()->set(key.substr(1, key.size() - 1), value);
     return;
   }
-  std::unique_lock storage_lock(storage_mutex_);
+  std::shared_lock storage_lock(storage_mutex_);
 
   // check local storage
   auto it = storage_.find(key);
@@ -300,8 +297,10 @@ inline void Blackboard::set(const std::string& key, const T& value)
                         GetAnyFromStringFunctor<T>());
       entry = createEntryImpl(key, new_port);
     }
-    storage_lock.lock();
 
+    // Lock entry_mutex before writing to prevent data races with
+    // concurrent readers (BUG-1/BUG-8 fix).
+    std::scoped_lock entry_lock(entry->entry_mutex);
     entry->value = new_value;
     entry->sequence_id++;
     entry->stamp = std::chrono::steady_clock::now().time_since_epoch();
@@ -310,8 +309,11 @@ inline void Blackboard::set(const std::string& key, const T& value)
   {
     // this is not the first time we set this entry, we need to check
     // if the type is the same or not.
-    Entry& entry = *it->second;
+    // Copy shared_ptr to prevent use-after-free if another thread
+    // calls unset() while we hold the reference (BUG-2 fix).
+    auto entry_ptr = it->second;
     storage_lock.unlock();
+    Entry& entry = *entry_ptr;
 
     std::scoped_lock scoped_lock(entry.entry_mutex);
 

include/behaviortree_cpp/controls/try_catch_node.h

@@ -0,0 +1,69 @@
+/* Copyright (C) 2018-2025 Davide Faconti, Eurecat -  All Rights Reserved
+*
+*   Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
+*   to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
+*   and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
+*   The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
+*
+*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
+*   WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#pragma once
+
+#include "behaviortree_cpp/control_node.h"
+
+namespace BT
+{
+/**
+ * @brief The TryCatch node executes children 1..N-1 as a Sequence ("try" block).
+ *
+ * If all children in the try-block succeed, this node returns SUCCESS.
+ *
+ * If any child in the try-block fails, the last child N is executed as a
+ * "catch" (cleanup) action, and this node returns FAILURE regardless of
+ * the catch child's result.
+ *
+ * - If a try-child returns RUNNING, this node returns RUNNING.
+ * - If a try-child returns SUCCESS, continue to the next try-child.
+ * - If a try-child returns FAILURE, enter catch mode and tick the last child.
+ * - If the catch child returns RUNNING, this node returns RUNNING.
+ * - When the catch child finishes (SUCCESS or FAILURE), this node returns FAILURE.
+ * - SKIPPED try-children are skipped over (not treated as failure).
+ *
+ * Port "catch_on_halt" (default false): if true, the catch child is also
+ * executed when the TryCatch node is halted while the try-block is RUNNING.
+ *
+ * Requires at least 2 children.
+ */
+class TryCatchNode : public ControlNode
+{
+public:
+  TryCatchNode(const std::string& name, const NodeConfig& config);
+
+  ~TryCatchNode() override = default;
+
+  TryCatchNode(const TryCatchNode&) = delete;
+  TryCatchNode& operator=(const TryCatchNode&) = delete;
+  TryCatchNode(TryCatchNode&&) = delete;
+  TryCatchNode& operator=(TryCatchNode&&) = delete;
+
+  static PortsList providedPorts()
+  {
+    return { InputPort<bool>("catch_on_halt", false,
+                             "If true, execute the catch child when "
+                             "the node is halted during the try-block") };
+  }
+
+  virtual void halt() override;
+
+private:
+  size_t current_child_idx_;
+  size_t skipped_count_;
+  bool in_catch_;
+
+  virtual BT::NodeStatus tick() override;
+};
+
+}  // namespace BT

src/blackboard.cpp

@@ -60,7 +60,7 @@ Blackboard::getEntry(const std::string& key) const
   }
 
   {
-    const std::unique_lock<std::mutex> storage_lock(storage_mutex_);
+    const std::shared_lock<std::shared_mutex> storage_lock(storage_mutex_);
     auto it = storage_.find(key);
     if(it != storage_.end())
     {
@@ -103,6 +103,9 @@ void Blackboard::addSubtreeRemapping(StringView internal, StringView external)
 
 void Blackboard::debugMessage() const
 {
+  // Lock storage_mutex_ (shared) to prevent iterator invalidation from
+  // concurrent modifications (BUG-5 fix).
+  const std::shared_lock<std::shared_mutex> storage_lock(storage_mutex_);
   for(const auto& [key, entry] : storage_)
   {
     auto port_type = entry->info.type();
@@ -136,6 +139,9 @@ void Blackboard::debugMessage() const
 
 std::vector<StringView> Blackboard::getKeys() const
 {
+  // Lock storage_mutex_ (shared) to prevent iterator invalidation and
+  // dangling StringView from concurrent modifications (BUG-6 fix).
+  const std::shared_lock<std::shared_mutex> storage_lock(storage_mutex_);
   if(storage_.empty())
   {
     return {};
@@ -151,15 +157,10 @@ std::vector<StringView> Blackboard::getKeys() const
 
 void Blackboard::clear()
 {
-  const std::unique_lock<std::mutex> storage_lock(storage_mutex_);
+  const std::unique_lock<std::shared_mutex> storage_lock(storage_mutex_);
   storage_.clear();
 }
 
-std::recursive_mutex& Blackboard::entryMutex() const
-{
-  return entry_mutex_;
-}
-
 void Blackboard::createEntry(const std::string& key, const TypeInfo& info)
 {
   if(StartWith(key, '@'))
@@ -178,49 +179,94 @@ void Blackboard::createEntry(const std::string& key, const TypeInfo& info)
 
 void Blackboard::cloneInto(Blackboard& dst) const
 {
-  // Lock both mutexes without risking lock-order inversion.
-  std::unique_lock<std::mutex> lk1(storage_mutex_, std::defer_lock);
-  std::unique_lock<std::mutex> lk2(dst.storage_mutex_, std::defer_lock);
-  std::lock(lk1, lk2);
-
-  // keys that are not updated must be removed.
-  std::unordered_set<std::string> keys_to_remove;
-  auto& dst_storage = dst.storage_;
-  for(const auto& [key, entry] : dst_storage)
+  // We must never hold storage_mutex_ while locking entry_mutex, because
+  // the script evaluation path (ExprAssignment::evaluate) does the reverse:
+  // entry_mutex → storage_mutex_ (via entryInfo → getEntry).
+  //
+  // Strategy: collect shared_ptrs under storage_mutex_, release it,
+  // then copy entry data under entry_mutex only.
+
+  struct CopyTask
   {
-    std::ignore = entry;  // unused in this loop
-    keys_to_remove.insert(key);
-  }
+    std::string key;
+    std::shared_ptr<Entry> src;
+    std::shared_ptr<Entry> dst;  // nullptr when a new entry is needed
+  };
 
-  // update or create entries in dst_storage
-  for(const auto& [src_key, src_entry] : storage_)
+  std::vector<CopyTask> tasks;
+  std::vector<std::string> keys_to_remove;
+
+  // Step 1: snapshot src/dst entries under both storage_mutex_ locks.
   {
-    keys_to_remove.erase(src_key);
+    std::shared_lock<std::shared_mutex> lk1(storage_mutex_, std::defer_lock);
+    std::unique_lock<std::shared_mutex> lk2(dst.storage_mutex_, std::defer_lock);
+    std::lock(lk1, lk2);
 
-    auto it = dst_storage.find(src_key);
-    if(it != dst_storage.end())
+    std::unordered_set<std::string> dst_keys;
+    for(const auto& [key, entry] : dst.storage_)
     {
-      // overwrite
-      auto& dst_entry = it->second;
-      dst_entry->string_converter = src_entry->string_converter;
-      dst_entry->value = src_entry->value;
-      dst_entry->info = src_entry->info;
-      dst_entry->sequence_id++;
-      dst_entry->stamp = std::chrono::steady_clock::now().time_since_epoch();
+      dst_keys.insert(key);
+    }
+
+    for(const auto& [src_key, src_entry] : storage_)
+    {
+      dst_keys.erase(src_key);
+      auto it = dst.storage_.find(src_key);
+      if(it != dst.storage_.end())
+      {
+        tasks.push_back({ src_key, src_entry, it->second });
+      }
+      else
+      {
+        tasks.push_back({ src_key, src_entry, nullptr });
+      }
+    }
+
+    for(const auto& key : dst_keys)
+    {
+      keys_to_remove.push_back(key);
+    }
+  }
+  // storage_mutex_ locks released here
+
+  // Step 2: copy entry data under entry_mutex only (BUG-3 fix).
+  std::vector<std::pair<std::string, std::shared_ptr<Entry>>> new_entries;
+
+  for(auto& task : tasks)
+  {
+    if(task.dst)
+    {
+      // overwrite existing entry
+      std::scoped_lock entry_locks(task.src->entry_mutex, task.dst->entry_mutex);
+      task.dst->string_converter = task.src->string_converter;
+      task.dst->value = task.src->value;
+      task.dst->info = task.src->info;
+      task.dst->sequence_id++;
+      task.dst->stamp = std::chrono::steady_clock::now().time_since_epoch();
     }
     else
     {
-      // create new
-      auto new_entry = std::make_shared<Entry>(src_entry->info);
-      new_entry->value = src_entry->value;
-      new_entry->string_converter = src_entry->string_converter;
-      dst_storage.insert({ src_key, new_entry });
+      // create new entry from src
+      std::scoped_lock src_lock(task.src->entry_mutex);
+      auto new_entry = std::make_shared<Entry>(task.src->info);
+      new_entry->value = task.src->value;
+      new_entry->string_converter = task.src->string_converter;
+      new_entries.emplace_back(task.key, std::move(new_entry));
     }
   }
 
-  for(const auto& key : keys_to_remove)
+  // Step 3: insert new entries and remove stale ones under dst.storage_mutex_.
+  if(!new_entries.empty() || !keys_to_remove.empty())
   {
-    dst_storage.erase(key);
+    const std::unique_lock<std::shared_mutex> dst_lock(dst.storage_mutex_);
+    for(auto& [key, entry] : new_entries)
+    {
+      dst.storage_.insert({ key, std::move(entry) });
+    }
+    for(const auto& key : keys_to_remove)
+    {
+      dst.storage_.erase(key);
+    }
   }
 }
 
@@ -236,7 +282,7 @@ Blackboard::Ptr Blackboard::parent()
 std::shared_ptr<Blackboard::Entry> Blackboard::createEntryImpl(const std::string& key,
                                                                const TypeInfo& info)
 {
-  const std::unique_lock<std::mutex> storage_lock(storage_mutex_);
+  const std::unique_lock<std::shared_mutex> storage_lock(storage_mutex_);
   // This function might be called recursively, when we do remapping, because we move
   // to the top scope to find already existing  entries
 
@@ -317,6 +363,8 @@ void ImportBlackboardFromJSON(const nlohmann::json& json, Blackboard& blackboard
         blackboard.createEntry(it.key(), res->second);
         entry = blackboard.getEntry(it.key());
       }
+      // Lock entry_mutex before writing to prevent data races (BUG-4 fix).
+      std::scoped_lock lk(entry->entry_mutex);
       entry->value = res->first;
     }
   }

src/bt_factory.cpp

@@ -131,6 +131,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl)
   registerNodeType<ReactiveFallback>("ReactiveFallback");
   registerNodeType<IfThenElseNode>("IfThenElse");
   registerNodeType<WhileDoElseNode>("WhileDoElse");
+  registerNodeType<TryCatchNode>("TryCatch");
 
   registerNodeType<InverterNode>("Inverter");