diff --git a/CMakeLists.txt b/CMakeLists.txt index 23fd11d07..12b0b977a 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index ebeebeda6..e7373d4dc 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/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" diff --git a/include/behaviortree_cpp/blackboard.h b/include/behaviortree_cpp/blackboard.h index 7229b5921..c88924400 100644 --- a/include/behaviortree_cpp/blackboard.h +++ b/include/behaviortree_cpp/blackboard.h @@ -9,6 +9,7 @@ #include #include +#include #include #include @@ -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 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> storage_; std::weak_ptr parent_bb_; std::unordered_map 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()); 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); diff --git a/include/behaviortree_cpp/controls/try_catch_node.h b/include/behaviortree_cpp/controls/try_catch_node.h new file mode 100644 index 000000000..ee711428e --- /dev/null +++ b/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("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 diff --git a/src/blackboard.cpp b/src/blackboard.cpp index ad9db06e6..adb8e2fd5 100644 --- a/src/blackboard.cpp +++ b/src/blackboard.cpp @@ -60,7 +60,7 @@ Blackboard::getEntry(const std::string& key) const } { - const std::unique_lock storage_lock(storage_mutex_); + const std::shared_lock 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 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 Blackboard::getKeys() const { + // Lock storage_mutex_ (shared) to prevent iterator invalidation and + // dangling StringView from concurrent modifications (BUG-6 fix). + const std::shared_lock storage_lock(storage_mutex_); if(storage_.empty()) { return {}; @@ -151,15 +157,10 @@ std::vector Blackboard::getKeys() const void Blackboard::clear() { - const std::unique_lock storage_lock(storage_mutex_); + const std::unique_lock 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 lk1(storage_mutex_, std::defer_lock); - std::unique_lock lk2(dst.storage_mutex_, std::defer_lock); - std::lock(lk1, lk2); - - // keys that are not updated must be removed. - std::unordered_set 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 src; + std::shared_ptr 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 tasks; + std::vector keys_to_remove; + + // Step 1: snapshot src/dst entries under both storage_mutex_ locks. { - keys_to_remove.erase(src_key); + std::shared_lock lk1(storage_mutex_, std::defer_lock); + std::unique_lock 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 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>> 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(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(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 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::createEntryImpl(const std::string& key, const TypeInfo& info) { - const std::unique_lock storage_lock(storage_mutex_); + const std::unique_lock 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; } } diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 432ae6abd..42cf43307 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -131,6 +131,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType("ReactiveFallback"); registerNodeType("IfThenElse"); registerNodeType("WhileDoElse"); + registerNodeType("TryCatch"); registerNodeType("Inverter"); diff --git a/src/controls/try_catch_node.cpp b/src/controls/try_catch_node.cpp new file mode 100644 index 000000000..2ddffbe5d --- /dev/null +++ b/src/controls/try_catch_node.cpp @@ -0,0 +1,137 @@ +/* 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. +*/ + +#include "behaviortree_cpp/controls/try_catch_node.h" + +namespace BT +{ +TryCatchNode::TryCatchNode(const std::string& name, const NodeConfig& config) + : ControlNode::ControlNode(name, config) + , current_child_idx_(0) + , skipped_count_(0) + , in_catch_(false) +{ + setRegistrationID("TryCatch"); +} + +void TryCatchNode::halt() +{ + bool catch_on_halt = false; + getInput("catch_on_halt", catch_on_halt); + + // If catch_on_halt is enabled and we were in the try-block (not already in catch), + // execute the catch child synchronously before halting. + if(catch_on_halt && !in_catch_ && isStatusActive(status()) && + children_nodes_.size() >= 2) + { + // Halt all try-block children first + for(size_t i = 0; i < children_nodes_.size() - 1; i++) + { + haltChild(i); + } + + // Tick the catch child. If it returns RUNNING, halt it too + // (best-effort cleanup during halt). + TreeNode* catch_child = children_nodes_.back(); + const NodeStatus catch_status = catch_child->executeTick(); + if(catch_status == NodeStatus::RUNNING) + { + haltChild(children_nodes_.size() - 1); + } + } + + current_child_idx_ = 0; + skipped_count_ = 0; + in_catch_ = false; + ControlNode::halt(); +} + +NodeStatus TryCatchNode::tick() +{ + const size_t children_count = children_nodes_.size(); + + if(children_count < 2) + { + throw LogicError("[", name(), "]: TryCatch requires at least 2 children"); + } + + if(!isStatusActive(status())) + { + skipped_count_ = 0; + in_catch_ = false; + } + + setStatus(NodeStatus::RUNNING); + + const size_t try_count = children_count - 1; + + // If we are in catch mode, tick the last child (cleanup) + if(in_catch_) + { + TreeNode* catch_child = children_nodes_.back(); + const NodeStatus catch_status = catch_child->executeTick(); + + if(catch_status == NodeStatus::RUNNING) + { + return NodeStatus::RUNNING; + } + + // Catch child finished (SUCCESS or FAILURE): return FAILURE + resetChildren(); + current_child_idx_ = 0; + in_catch_ = false; + return NodeStatus::FAILURE; + } + + // Try-block: execute children 0..N-2 as a Sequence + while(current_child_idx_ < try_count) + { + TreeNode* current_child_node = children_nodes_[current_child_idx_]; + const NodeStatus child_status = current_child_node->executeTick(); + + switch(child_status) + { + case NodeStatus::RUNNING: { + return NodeStatus::RUNNING; + } + case NodeStatus::FAILURE: { + // Enter catch mode: halt try-block children, then tick catch child + resetChildren(); + current_child_idx_ = 0; + in_catch_ = true; + return tick(); // re-enter to tick the catch child + } + case NodeStatus::SUCCESS: { + current_child_idx_++; + } + break; + case NodeStatus::SKIPPED: { + current_child_idx_++; + skipped_count_++; + } + break; + case NodeStatus::IDLE: { + throw LogicError("[", name(), "]: A child should not return IDLE"); + } + } + } + + // All try-children completed successfully (or were skipped) + const bool all_skipped = (skipped_count_ == try_count); + resetChildren(); + current_child_idx_ = 0; + skipped_count_ = 0; + + return all_skipped ? NodeStatus::SKIPPED : NodeStatus::SUCCESS; +} + +} // namespace BT diff --git a/src/xml_parsing.cpp b/src/xml_parsing.cpp index fbe1d74e5..c72aca0cb 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -293,6 +293,11 @@ void BT::XMLParser::PImpl::loadSubtreeModel(const XMLElement* xml_root) sub_node = sub_node->NextSiblingElement("SubTree")) { auto subtree_id = sub_node->Attribute("ID"); + if(subtree_id == nullptr) + { + throw RuntimeError("Missing attribute 'ID' in SubTree element " + "within TreeNodesModel"); + } auto& subtree_model = subtree_models[subtree_id]; const std::pair port_types[3] = { @@ -307,13 +312,13 @@ void BT::XMLParser::PImpl::loadSubtreeModel(const XMLElement* xml_root) port_node = port_node->NextSiblingElement(name)) { BT::PortInfo port(direction); - auto name = port_node->Attribute("name"); - if(name == nullptr) + auto port_name = port_node->Attribute("name"); + if(port_name == nullptr) { throw RuntimeError("Missing attribute [name] in port (SubTree model)"); } // Validate port name - validatePortName(name, port_node->GetLineNum()); + validatePortName(port_name, port_node->GetLineNum()); if(auto default_value = port_node->Attribute("default")) { port.setDefaultValue(default_value); @@ -322,7 +327,7 @@ void BT::XMLParser::PImpl::loadSubtreeModel(const XMLElement* xml_root) { port.setDescription(description); } - subtree_model.ports[name] = std::move(port); + subtree_model.ports[port_name] = std::move(port); } } } @@ -627,6 +632,11 @@ void VerifyXML(const std::string& xml_text, ThrowError(line_number, std::string("The node '") + registered_name + "' must have 1 or more children"); } + if(registered_name == "TryCatch" && children_count < 2) + { + ThrowError(line_number, std::string("The node 'TryCatch' must have " + "at least 2 children")); + } if(registered_name == "ReactiveSequence") { size_t async_count = 0; diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 7c5418c1d..a009ee5ea 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -46,6 +46,7 @@ set(BT_TESTS gtest_subtree.cpp gtest_switch.cpp gtest_tree.cpp + gtest_try_catch.cpp gtest_exception_tracking.cpp gtest_updates.cpp gtest_wakeup.cpp @@ -54,6 +55,8 @@ set(BT_TESTS gtest_simple_string.cpp gtest_polymorphic_ports.cpp gtest_plugin_issue953.cpp + gtest_blackboard_thread_safety.cpp + gtest_xml_null_subtree_id.cpp script_parser_test.cpp test_helper.hpp diff --git a/tests/gtest_blackboard_thread_safety.cpp b/tests/gtest_blackboard_thread_safety.cpp new file mode 100644 index 000000000..483720c8a --- /dev/null +++ b/tests/gtest_blackboard_thread_safety.cpp @@ -0,0 +1,337 @@ +/* Copyright (C) 2024 Davide Faconti - All Rights Reserved + * + * Tests for Blackboard thread-safety bugs. + * These tests reproduce data races that are detectable by ThreadSanitizer (TSan). + * Under normal (non-TSan) builds, they exercise the racy code paths but may + * not always crash — TSan is required for reliable detection. + */ + +#include "behaviortree_cpp/blackboard.h" + +#include +#include +#include +#include + +#include + +using namespace BT; + +// BUG-2: Blackboard::set() existing-entry path takes a raw reference to +// the Entry, then unlocks storage_mutex_. If another thread calls unset() +// on the same key in that window, the shared_ptr in the map is erased +// and the Entry may be destroyed, leaving a dangling reference. +// +// This test hammers concurrent set() + unset() on the same key. +// Under TSan it will report a data race / use-after-free before the fix. +TEST(BlackboardThreadSafety, SetAndUnsetRace_Bug2) +{ + auto bb = Blackboard::create(); + + // Pre-create the entry so set() takes the existing-entry branch + bb->set("key", 0); + + std::atomic stop{ false }; + constexpr int kIterations = 5000; + + auto setter = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + // This creates the entry if it was unset, or updates it + bb->set("key", i); + } + }; + + auto unsetter = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + bb->unset("key"); + } + }; + + std::thread t1(setter); + std::thread t2(unsetter); + + t1.join(); + t2.join(); + + // If we get here without crashing/TSan error, the test passes + SUCCEED(); +} + +// BUG-1 + BUG-8: Blackboard::set() new-entry path. +// After createEntryImpl() inserts the entry into storage_, the code writes +// entry->value, entry->sequence_id, entry->stamp WITHOUT holding entry_mutex. +// The entry is already visible to other threads via getEntry(). +// +// The race is between set()'s unlocked write (lines 305-307) and a reader +// that obtained a shared_ptr via getEntry() and reads entry members +// under entry_mutex. Since set() doesn't hold entry_mutex, TSan detects the race. +// +// Strategy: pre-create entries so getEntry() works, then concurrently +// unset+set (to force new-entry path) while reader holds the entry. +TEST(BlackboardThreadSafety, SetNewEntryWhileReading_Bug1_Bug8) +{ + constexpr int kIterations = 2000; + + auto bb = Blackboard::create(); + + std::atomic stop{ false }; + + // Writer: keeps cycling between unset and set to trigger the new-entry path + auto writer = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + std::string key = "key_" + std::to_string(i % 5); + bb->unset(key); + bb->set(key, i); + } + }; + + // Reader: grabs the entry via getEntry() and reads members under entry_mutex. + // The race is that set() writes entry->value WITHOUT entry_mutex after + // createEntryImpl() inserts the entry. + auto reader = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + std::string key = "key_" + std::to_string(i % 5); + auto entry = bb->getEntry(key); + if(entry) + { + std::unique_lock lk(entry->entry_mutex); + volatile bool empty = entry->value.empty(); + volatile auto seq = entry->sequence_id; + (void)empty; + (void)seq; + } + } + }; + + std::thread t1(writer); + std::thread t2(reader); + + t1.join(); + t2.join(); + + SUCCEED(); +} + +// BUG-8 specifically: Two threads calling set() for the same NEW key. +// Both see key as missing, both call createEntryImpl(), then both write +// entry->value without entry_mutex. +TEST(BlackboardThreadSafety, TwoThreadsSetSameNewKey_Bug8) +{ + constexpr int kRounds = 500; + + for(int round = 0; round < kRounds; round++) + { + auto bb = Blackboard::create(); + std::string key = "new_key"; + + // Use a simple latch so both threads call set() at nearly the same time + std::mutex mtx; + std::condition_variable cv; + int ready_count = 0; + + auto thread_func = [&](int value) { + { + std::unique_lock lk(mtx); + ready_count++; + cv.notify_all(); + cv.wait(lk, [&] { return ready_count >= 2; }); + } + bb->set(key, value); + }; + + std::thread t1(thread_func, 1); + std::thread t2(thread_func, 2); + + t1.join(); + t2.join(); + + // The value should be one of the two written values + int result = bb->get(key); + ASSERT_TRUE(result == 1 || result == 2); + } +} + +// BUG-3: cloneInto() reads/writes entry members (value, info, string_converter, +// sequence_id, stamp) while holding storage_mutex_ but NOT entry_mutex. +// A concurrent thread that already holds a shared_ptr (obtained +// before cloneInto starts) can read entry members under entry_mutex, +// which doesn't synchronize with storage_mutex_. +// +// Strategy: grab entry shared_ptrs first, then start cloneInto in parallel +// with reads under entry_mutex. +TEST(BlackboardThreadSafety, CloneIntoWhileReading_Bug3) +{ + auto src = Blackboard::create(); + auto dst = Blackboard::create(); + + constexpr int kEntries = 20; + + // Pre-populate both blackboards + for(int i = 0; i < kEntries; i++) + { + std::string key = "key_" + std::to_string(i); + src->set(key, i); + dst->set(key, i * 10); + } + + // Pre-grab entry shared_ptrs from dst so reader doesn't need storage_mutex_ + std::vector> dst_entries; + for(int i = 0; i < kEntries; i++) + { + dst_entries.push_back(dst->getEntry("key_" + std::to_string(i))); + } + + constexpr int kIterations = 2000; + std::atomic stop{ false }; + + auto cloner = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + src->cloneInto(*dst); + } + }; + + // Reader holds pre-obtained entry shared_ptrs and reads under entry_mutex + auto reader = [&]() { + for(int i = 0; i < kIterations && !stop; i++) + { + auto& entry = dst_entries[i % kEntries]; + std::unique_lock lk(entry->entry_mutex); + volatile bool empty = entry->value.empty(); + volatile auto seq = entry->sequence_id; + (void)empty; + (void)seq; + } + }; + + std::thread t1(cloner); + std::thread t2(reader); + + t1.join(); + t2.join(); + + SUCCEED(); +} + +// BUG-4: ImportBlackboardFromJSON writes entry->value without entry_mutex. +// The reader holds a pre-obtained shared_ptr and reads under entry_mutex, +// which doesn't synchronize with ImportBlackboardFromJSON's unprotected write. +TEST(BlackboardThreadSafety, ImportJsonWhileReading_Bug4) +{ + auto bb = Blackboard::create(); + + // Pre-populate + bb->set("int_val", 42); + bb->set("str_val", std::string("hello")); + + // Export to JSON + auto json = ExportBlackboardToJSON(*bb); + + // Pre-grab entry so reader doesn't need storage_mutex_ + auto entry = bb->getEntry("int_val"); + + constexpr int kIterations = 2000; + + auto importer = [&]() { + for(int i = 0; i < kIterations; i++) + { + ImportBlackboardFromJSON(json, *bb); + } + }; + + // Reader holds pre-obtained entry and reads under entry_mutex + auto reader = [&]() { + for(int i = 0; i < kIterations; i++) + { + std::unique_lock lk(entry->entry_mutex); + volatile bool empty = entry->value.empty(); + (void)empty; + } + }; + + std::thread t1(importer); + std::thread t2(reader); + + t1.join(); + t2.join(); + + SUCCEED(); +} + +// BUG-5: debugMessage() iterates storage_ without holding storage_mutex_. +// Concurrent modification (insert/erase) causes iterator invalidation / UB. +TEST(BlackboardThreadSafety, DebugMessageWhileModifying_Bug5) +{ + auto bb = Blackboard::create(); + + constexpr int kIterations = 500; + + auto modifier = [&]() { + for(int i = 0; i < kIterations; i++) + { + std::string key = "key_" + std::to_string(i % 50); + bb->set(key, i); + if(i % 3 == 0) + { + bb->unset(key); + } + } + }; + + auto debugger = [&]() { + for(int i = 0; i < kIterations; i++) + { + // Suppress stdout noise + testing::internal::CaptureStdout(); + bb->debugMessage(); + testing::internal::GetCapturedStdout(); + } + }; + + std::thread t1(modifier); + std::thread t2(debugger); + + t1.join(); + t2.join(); + + SUCCEED(); +} + +// BUG-6: getKeys() iterates storage_ without holding storage_mutex_. +// Also returns StringView into map keys which can dangle if entries are erased. +TEST(BlackboardThreadSafety, GetKeysWhileModifying_Bug6) +{ + auto bb = Blackboard::create(); + + constexpr int kIterations = 1000; + + auto modifier = [&]() { + for(int i = 0; i < kIterations; i++) + { + std::string key = "key_" + std::to_string(i % 50); + bb->set(key, i); + } + }; + + auto key_reader = [&]() { + for(int i = 0; i < kIterations; i++) + { + auto keys = bb->getKeys(); + // Just access the keys to detect any race + volatile size_t count = keys.size(); + (void)count; + } + }; + + std::thread t1(modifier); + std::thread t2(key_reader); + + t1.join(); + t2.join(); + + SUCCEED(); +} diff --git a/tests/gtest_try_catch.cpp b/tests/gtest_try_catch.cpp new file mode 100644 index 000000000..7ad3e1feb --- /dev/null +++ b/tests/gtest_try_catch.cpp @@ -0,0 +1,466 @@ +/* Copyright (C) 2025 Davide Faconti - 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. +*/ + +#include "test_helper.hpp" + +#include "behaviortree_cpp/bt_factory.h" + +#include + +using BT::NodeStatus; + +class TryCatchTest : public testing::Test +{ +protected: + BT::BehaviorTreeFactory factory; + std::array counters; + + void SetUp() override + { + RegisterTestTick(factory, "Test", counters); + } +}; + +TEST_F(TryCatchTest, AllTryChildrenSucceed) +{ + const std::string xml_text = R"( + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 1); // TestA executed + ASSERT_EQ(counters[1], 1); // TestB executed + ASSERT_EQ(counters[2], 0); // TestC (catch) NOT executed +} + +TEST_F(TryCatchTest, FirstChildFails_CatchExecuted) +{ + const std::string xml_text = R"( + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::FAILURE); + ASSERT_EQ(counters[0], 0); // TestA NOT executed (after failed child) + ASSERT_EQ(counters[1], 1); // TestB (catch) executed +} + +TEST_F(TryCatchTest, SecondChildFails_CatchExecuted) +{ + const std::string xml_text = R"( + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::FAILURE); + ASSERT_EQ(counters[0], 1); // TestA executed (before failure) + ASSERT_EQ(counters[1], 1); // TestB (catch) executed +} + +TEST_F(TryCatchTest, CatchReturnsFailure_NodeStillReturnsFAILURE) +{ + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::FAILURE); +} + +TEST_F(TryCatchTest, CatchReturnsSuccess_NodeStillReturnsFAILURE) +{ + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + // Even if catch succeeds, TryCatch returns FAILURE + ASSERT_EQ(status, NodeStatus::FAILURE); +} + +TEST_F(TryCatchTest, TryChildRunning) +{ + int tick_count = 0; + factory.registerSimpleCondition("RunningThenSuccess", [&tick_count](BT::TreeNode&) { + tick_count++; + if(tick_count == 1) + { + return NodeStatus::RUNNING; + } + return NodeStatus::SUCCESS; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING); + + status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 0); // Catch NOT executed +} + +TEST_F(TryCatchTest, CatchChildRunning) +{ + int catch_tick_count = 0; + factory.registerSimpleCondition("RunningThenFailure", + [&catch_tick_count](BT::TreeNode&) { + catch_tick_count++; + if(catch_tick_count == 1) + { + return NodeStatus::RUNNING; + } + return NodeStatus::FAILURE; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + // First tick: try fails, catch starts and returns RUNNING + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING); + + // Second tick: catch returns FAILURE, TryCatch returns FAILURE + status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::FAILURE); +} + +TEST_F(TryCatchTest, MinimumTwoChildren_ParseTimeValidation) +{ + const std::string xml_text = R"( + + + + + + + )"; + + // Error should be caught at parse time, not tick time + ASSERT_THROW(factory.createTreeFromText(xml_text), BT::RuntimeError); +} + +TEST_F(TryCatchTest, ReExecuteAfterSuccess) +{ + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + auto status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 1); + + tree.haltTree(); + status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 2); // TestA executed again + ASSERT_EQ(counters[1], 0); // Catch still never executed +} + +TEST_F(TryCatchTest, ReExecuteAfterFailure) +{ + int try_tick_count = 0; + factory.registerSimpleAction("FailThenSucceed", [&try_tick_count](BT::TreeNode&) { + try_tick_count++; + if(try_tick_count == 1) + { + return NodeStatus::FAILURE; + } + return NodeStatus::SUCCESS; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + // First execution: try fails, catch runs + auto status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::FAILURE); + ASSERT_EQ(counters[0], 1); // Catch executed + + // Second execution: try succeeds + tree.haltTree(); + status = tree.tickWhileRunning(); + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 1); // Catch not executed again +} + +TEST_F(TryCatchTest, CatchOnHalt_Disabled) +{ + int catch_count = 0; + factory.registerSimpleAction("CountCatch", [&catch_count](BT::TreeNode&) { + catch_count++; + return NodeStatus::SUCCESS; + }); + + int try_ticks = 0; + factory.registerSimpleCondition("AlwaysRunning", [&try_ticks](BT::TreeNode&) { + try_ticks++; + return NodeStatus::RUNNING; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING); + + // Halt while try-block is RUNNING; catch_on_halt defaults to false + tree.haltTree(); + ASSERT_EQ(catch_count, 0); // Catch NOT executed on halt +} + +TEST_F(TryCatchTest, CatchOnHalt_Enabled) +{ + int catch_count = 0; + factory.registerSimpleAction("CountCatch", [&catch_count](BT::TreeNode&) { + catch_count++; + return NodeStatus::SUCCESS; + }); + + int try_ticks = 0; + factory.registerSimpleCondition("AlwaysRunning", [&try_ticks](BT::TreeNode&) { + try_ticks++; + return NodeStatus::RUNNING; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING); + + // Halt while try-block is RUNNING; catch_on_halt is true + tree.haltTree(); + ASSERT_EQ(catch_count, 1); // Catch executed on halt +} + +TEST_F(TryCatchTest, CatchOnHalt_NotTriggeredWhenAlreadyInCatch) +{ + int catch_ticks = 0; + factory.registerSimpleCondition("RunningCatch", [&catch_ticks](BT::TreeNode&) { + catch_ticks++; + return NodeStatus::RUNNING; + }); + + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + // First tick: try fails, enters catch, catch returns RUNNING + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING); + ASSERT_EQ(catch_ticks, 1); + + // Halt while in catch mode: should NOT re-trigger catch + tree.haltTree(); + ASSERT_EQ(catch_ticks, 1); // Catch NOT ticked again +} + +TEST_F(TryCatchTest, AsyncCatchCompletesInsideSequence) +{ + // The catch child returns RUNNING for 5 ticks, then SUCCESS. + // Verify that the Sequence keeps ticking TryCatch, which keeps + // ticking the catch child until it completes. + const int kRunningTicks = 5; + int catch_ticks = 0; + factory.registerSimpleCondition("AsyncCleanup", + [&catch_ticks, kRunningTicks](BT::TreeNode&) { + catch_ticks++; + if(catch_ticks <= kRunningTicks) + { + return NodeStatus::RUNNING; + } + return NodeStatus::SUCCESS; + }); + + const std::string xml_text = R"( + + + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + // Tick-by-tick: the tree should stay RUNNING while catch is async + for(int i = 0; i < kRunningTicks; i++) + { + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::RUNNING) << "Expected RUNNING on tick " << (i + 1); + ASSERT_EQ(catch_ticks, i + 1); + } + + // Next tick: catch completes → TryCatch returns FAILURE → Sequence returns FAILURE + auto status = tree.tickOnce(); + ASSERT_EQ(status, NodeStatus::FAILURE); + + // Catch child was ticked exactly kRunningTicks + 1 times (5 RUNNING + 1 SUCCESS) + ASSERT_EQ(catch_ticks, kRunningTicks + 1); + + // TestA was never reached because TryCatch returned FAILURE + ASSERT_EQ(counters[0], 0); +} + +TEST_F(TryCatchTest, SingleTryChild_Success) +{ + const std::string xml_text = R"( + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::SUCCESS); + ASSERT_EQ(counters[0], 1); + ASSERT_EQ(counters[1], 0); +} + +TEST_F(TryCatchTest, ManyTryChildren_ThirdFails) +{ + const std::string xml_text = R"( + + + + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + auto status = tree.tickWhileRunning(); + + ASSERT_EQ(status, NodeStatus::FAILURE); + ASSERT_EQ(counters[0], 1); // TestA executed + ASSERT_EQ(counters[1], 1); // TestB executed + ASSERT_EQ(counters[2], 1); // TestC (catch) executed +} diff --git a/tests/gtest_xml_null_subtree_id.cpp b/tests/gtest_xml_null_subtree_id.cpp new file mode 100644 index 000000000..4110ed3a6 --- /dev/null +++ b/tests/gtest_xml_null_subtree_id.cpp @@ -0,0 +1,37 @@ +/* Copyright (C) 2024 Davide Faconti - All Rights Reserved + * + * Test for BUG-7: loadSubtreeModel crashes on null subtree_id + * when a element in is missing the ID attribute. + */ + +#include "behaviortree_cpp/bt_factory.h" + +#include + +using namespace BT; + +// BUG-7: If a element inside is missing the +// "ID" attribute, Attribute("ID") returns nullptr, which is used as a +// key in std::unordered_map — undefined behavior (null pointer dereference). +// After the fix, this should throw a descriptive RuntimeError. +TEST(XMLParserTest, SubTreeModelMissingID_Bug7) +{ + const std::string xml_text = R"( + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + + // Before fix: crash (null pointer as map key) + // After fix: should throw RuntimeError about missing ID + EXPECT_THROW(factory.registerBehaviorTreeFromText(xml_text), RuntimeError); +}