diff --git a/CMakeLists.txt b/CMakeLists.txt index e69c9e96c..20271e93f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -134,6 +134,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 42b860f99..e0e1cfbf3 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -21,6 +21,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/if_then_else_node.h" #include "behaviortree_cpp/controls/while_do_else_node.h" 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..cb259ecfa --- /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") }; + } + + void halt() override; + +private: + size_t current_child_idx_ = 0; + size_t skipped_count_ = 0; + bool in_catch_ = false; + + BT::NodeStatus tick() override; +}; + +} // namespace BT diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 488bfba60..e7fc0ec71 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -55,6 +55,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..dca6c6c87 --- /dev/null +++ b/src/controls/try_catch_node.cpp @@ -0,0 +1,134 @@ +/* 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) +{ + 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 a25a53a6f..3d271af9a 100644 --- a/src/xml_parsing.cpp +++ b/src/xml_parsing.cpp @@ -504,6 +504,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 c00c5987b..3c2378429 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -26,6 +26,7 @@ set(BT_TESTS gtest_subtree.cpp gtest_switch.cpp gtest_tree.cpp + gtest_try_catch.cpp gtest_updates.cpp gtest_wakeup.cpp gtest_interface.cpp diff --git a/tests/gtest_try_catch.cpp b/tests/gtest_try_catch.cpp new file mode 100644 index 000000000..ff245e44e --- /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((void)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 +}