diff --git a/include/behaviortree_cpp/exceptions.h b/include/behaviortree_cpp/exceptions.h index 9bb2602ad..edc18340b 100644 --- a/include/behaviortree_cpp/exceptions.h +++ b/include/behaviortree_cpp/exceptions.h @@ -16,6 +16,7 @@ #include #include +#include #include "utils/strcat.hpp" @@ -67,6 +68,63 @@ class RuntimeError : public BehaviorTreeException {} }; +/// Information about a node in the tick backtrace. +struct TickBacktraceEntry +{ + std::string node_name; + std::string node_path; + std::string registration_name; +}; + +/// Exception thrown when a node's tick() method throws an exception. +/// Contains the originating node and full tick backtrace showing the path through the tree. +class NodeExecutionError : public RuntimeError +{ +public: + NodeExecutionError(std::vector backtrace, + const std::string& original_message) + : RuntimeError(formatMessage(backtrace, original_message)) + , backtrace_(std::move(backtrace)) + , original_message_(original_message) + {} + + /// The node that threw the exception (innermost in the backtrace) + [[nodiscard]] const TickBacktraceEntry& failedNode() const + { + return backtrace_.back(); + } + + /// Full tick backtrace from root to failing node + [[nodiscard]] const std::vector& backtrace() const + { + return backtrace_; + } + + [[nodiscard]] const std::string& originalMessage() const + { + return original_message_; + } + +private: + std::vector backtrace_; + std::string original_message_; + + static std::string formatMessage(const std::vector& bt, + const std::string& original_msg) + { + std::string msg = + StrCat("Exception in node '", bt.back().node_path, "': ", original_msg); + msg += "\nTick backtrace:"; + for(size_t i = 0; i < bt.size(); ++i) + { + const bool is_last = (i == bt.size() - 1); + msg += StrCat("\n ", is_last ? "-> " : " ", bt[i].node_path, " (", + bt[i].registration_name, ")"); + } + return msg; + } +}; + } // namespace BT #endif diff --git a/src/tree_node.cpp b/src/tree_node.cpp index fc35189c8..33723450b 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -16,10 +16,43 @@ #include #include #include +#include namespace BT { +// Thread-local stack tracking the current tick hierarchy. +// Used to build a backtrace when an exception is thrown during tick(). +static thread_local std::vector tick_stack_; + +// RAII guard to push/pop nodes from the tick stack +class TickStackGuard +{ +public: + explicit TickStackGuard(const TreeNode* node) + { + tick_stack_.push_back(node); + } + ~TickStackGuard() + { + tick_stack_.pop_back(); + } + TickStackGuard(const TickStackGuard&) = delete; + TickStackGuard& operator=(const TickStackGuard&) = delete; + + // Build a backtrace from the current tick stack + static std::vector buildBacktrace() + { + std::vector backtrace; + backtrace.reserve(tick_stack_.size()); + for(const auto* node : tick_stack_) + { + backtrace.push_back({ node->name(), node->fullPath(), node->registrationName() }); + } + return backtrace; + } +}; + struct TreeNode::PImpl { PImpl(std::string name, NodeConfig config) @@ -69,6 +102,9 @@ TreeNode::~TreeNode() = default; NodeStatus TreeNode::executeTick() { + // Track this node in the tick stack for exception backtrace + TickStackGuard stack_guard(this); + auto new_status = _p->status; PreTickCallback pre_tick; PostTickCallback post_tick; @@ -109,7 +145,20 @@ NodeStatus TreeNode::executeTick() // See issue #861 for details. const auto t1 = steady_clock::now(); std::atomic_thread_fence(std::memory_order_seq_cst); - new_status = tick(); + try + { + new_status = tick(); + } + catch(const NodeExecutionError&) + { + // Already wrapped by a child node, re-throw as-is to preserve original info + throw; + } + catch(const std::exception& ex) + { + // Wrap the exception with node context and backtrace + throw NodeExecutionError(TickStackGuard::buildBacktrace(), ex.what()); + } std::atomic_thread_fence(std::memory_order_seq_cst); const auto t2 = steady_clock::now(); if(monitor_tick) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index c7022c3f2..cd76a9468 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_exception_tracking.cpp gtest_updates.cpp gtest_wakeup.cpp gtest_while_do_else.cpp diff --git a/tests/gtest_exception_tracking.cpp b/tests/gtest_exception_tracking.cpp new file mode 100644 index 000000000..54b6a6826 --- /dev/null +++ b/tests/gtest_exception_tracking.cpp @@ -0,0 +1,237 @@ +/* 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/bt_factory.h" + +#include + +using namespace BT; + +// Test node that throws an exception +class ThrowingAction : public SyncActionNode +{ +public: + ThrowingAction(const std::string& name, const NodeConfig& config) + : SyncActionNode(name, config) + {} + + NodeStatus tick() override + { + throw std::runtime_error("Test exception from ThrowingAction"); + } + + static PortsList providedPorts() + { + return {}; + } +}; + +// Test node that succeeds +class SucceedingAction : public SyncActionNode +{ +public: + SucceedingAction(const std::string& name, const NodeConfig& config) + : SyncActionNode(name, config) + {} + + NodeStatus tick() override + { + return NodeStatus::SUCCESS; + } + + static PortsList providedPorts() + { + return {}; + } +}; + +TEST(ExceptionTracking, BasicExceptionCapture) +{ + // Simple tree: Sequence -> ThrowingAction + const char* xml = R"( + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("ThrowingAction"); + + auto tree = factory.createTreeFromText(xml); + + try + { + tree.tickOnce(); + FAIL() << "Expected NodeExecutionError to be thrown"; + } + catch(const NodeExecutionError& e) + { + // Verify the failed node info + EXPECT_EQ(e.failedNode().node_name, "thrower"); + EXPECT_EQ(e.failedNode().registration_name, "ThrowingAction"); + EXPECT_EQ(e.originalMessage(), "Test exception from ThrowingAction"); + + // Verify backtrace has the node + ASSERT_GE(e.backtrace().size(), 1u); + EXPECT_EQ(e.backtrace().back().node_name, "thrower"); + } +} + +TEST(ExceptionTracking, NestedExceptionBacktrace) +{ + // Tree: Sequence -> RetryNode -> ThrowingAction + // This tests that the backtrace shows the full path + const char* xml = R"( + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("ThrowingAction"); + factory.registerNodeType("SucceedingAction"); + + auto tree = factory.createTreeFromText(xml); + + try + { + tree.tickOnce(); + FAIL() << "Expected NodeExecutionError to be thrown"; + } + catch(const NodeExecutionError& e) + { + // Verify the failed node is the innermost throwing node + EXPECT_EQ(e.failedNode().node_name, "nested_thrower"); + + // Verify backtrace shows the full path (at least 3 nodes: Sequence, Retry, Thrower) + ASSERT_GE(e.backtrace().size(), 3u); + + // Check the what() message contains backtrace info + std::string what_msg = e.what(); + EXPECT_NE(what_msg.find("nested_thrower"), std::string::npos); + EXPECT_NE(what_msg.find("Tick backtrace"), std::string::npos); + } +} + +TEST(ExceptionTracking, SubtreeExceptionBacktrace) +{ + // Tree with subtree: MainTree -> Subtree -> ThrowingAction + const char* xml = R"( + + + + + + + + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("ThrowingAction"); + + auto tree = factory.createTreeFromText(xml); + + try + { + tree.tickOnce(); + FAIL() << "Expected NodeExecutionError to be thrown"; + } + catch(const NodeExecutionError& e) + { + // Verify the failed node is the one in the subtree + EXPECT_EQ(e.failedNode().node_name, "subtree_thrower"); + + // Verify fullPath includes the subtree hierarchy + std::string full_path = e.failedNode().node_path; + EXPECT_NE(full_path.find("subtree_thrower"), std::string::npos); + } +} + +TEST(ExceptionTracking, NoExceptionNoWrapping) +{ + // Verify that trees that don't throw work normally + const char* xml = R"( + + + + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("SucceedingAction"); + + auto tree = factory.createTreeFromText(xml); + + // Should not throw + EXPECT_NO_THROW({ + auto status = tree.tickOnce(); + EXPECT_EQ(status, NodeStatus::SUCCESS); + }); +} + +TEST(ExceptionTracking, BacktraceEntryContents) +{ + // Test that TickBacktraceEntry contains all expected fields + const char* xml = R"( + + + + + + )"; + + BehaviorTreeFactory factory; + factory.registerNodeType("ThrowingAction"); + + auto tree = factory.createTreeFromText(xml); + + try + { + tree.tickOnce(); + FAIL() << "Expected exception"; + } + catch(const NodeExecutionError& e) + { + const auto& entry = e.failedNode(); + // Check all fields are populated + EXPECT_FALSE(entry.node_name.empty()); + EXPECT_FALSE(entry.node_path.empty()); + EXPECT_FALSE(entry.registration_name.empty()); + + EXPECT_EQ(entry.node_name, "my_action"); + EXPECT_EQ(entry.registration_name, "ThrowingAction"); + } +} diff --git a/tests/gtest_if_then_else.cpp b/tests/gtest_if_then_else.cpp index b67b2910a..44fecf7c9 100644 --- a/tests/gtest_if_then_else.cpp +++ b/tests/gtest_if_then_else.cpp @@ -215,7 +215,8 @@ TEST_F(IfThenElseTest, InvalidChildCount_One) )"; auto tree = factory.createTreeFromText(xml_text); - ASSERT_THROW(tree.tickWhileRunning(), std::logic_error); + // Throws LogicError, wrapped in NodeExecutionError with backtrace + ASSERT_THROW(tree.tickWhileRunning(), BT::BehaviorTreeException); } TEST_F(IfThenElseTest, InvalidChildCount_Four) @@ -234,5 +235,6 @@ TEST_F(IfThenElseTest, InvalidChildCount_Four) )"; auto tree = factory.createTreeFromText(xml_text); - ASSERT_THROW(tree.tickWhileRunning(), std::logic_error); + // Throws LogicError, wrapped in NodeExecutionError with backtrace + ASSERT_THROW(tree.tickWhileRunning(), BT::BehaviorTreeException); } diff --git a/tests/gtest_port_type_rules.cpp b/tests/gtest_port_type_rules.cpp index 0a5fbd538..70b0f9f21 100644 --- a/tests/gtest_port_type_rules.cpp +++ b/tests/gtest_port_type_rules.cpp @@ -687,7 +687,8 @@ TEST(PortTypeRules, TypeLock_RuntimeTypeChange_Fails) std::this_thread::sleep_for(std::chrono::milliseconds{ 5 }); // Second tick fails (tries to change TestPoint to string) - EXPECT_THROW(tree.tickWhileRunning(), LogicError); + // Throws LogicError, wrapped in NodeExecutionError with backtrace + EXPECT_THROW(tree.tickWhileRunning(), BehaviorTreeException); } //============================================================================== diff --git a/tests/gtest_while_do_else.cpp b/tests/gtest_while_do_else.cpp index ba2f9c823..3b08644c3 100644 --- a/tests/gtest_while_do_else.cpp +++ b/tests/gtest_while_do_else.cpp @@ -234,7 +234,8 @@ TEST_F(WhileDoElseTest, InvalidChildCount_One) )"; auto tree = factory.createTreeFromText(xml_text); - ASSERT_THROW(tree.tickWhileRunning(), std::logic_error); + // Throws LogicError, wrapped in NodeExecutionError with backtrace + ASSERT_THROW(tree.tickWhileRunning(), BT::BehaviorTreeException); } TEST_F(WhileDoElseTest, InvalidChildCount_Four) @@ -253,7 +254,8 @@ TEST_F(WhileDoElseTest, InvalidChildCount_Four) )"; auto tree = factory.createTreeFromText(xml_text); - ASSERT_THROW(tree.tickWhileRunning(), std::logic_error); + // Throws LogicError, wrapped in NodeExecutionError with backtrace + ASSERT_THROW(tree.tickWhileRunning(), BT::BehaviorTreeException); } TEST_F(WhileDoElseTest, ConditionRunning)