From 44672ac1fd06eafa5988ba4b904874409f66d26e Mon Sep 17 00:00:00 2001 From: lisyarus Date: Fri, 13 Sep 2024 23:28:10 +0300 Subject: [PATCH] Add bt_assert macro for non-crashing assertions in behavior trees --- libs/bt/CMakeLists.txt | 2 +- libs/bt/include/psemek/bt/action.hpp | 11 +++++++++- libs/bt/include/psemek/bt/assert.hpp | 28 +++++++++++++++++++++++++ libs/bt/include/psemek/bt/by_index.hpp | 18 +++++++++++++++- libs/bt/include/psemek/bt/condition.hpp | 10 ++++++++- libs/bt/include/psemek/bt/lazy.hpp | 15 +++++++++++-- libs/bt/include/psemek/bt/node.hpp | 19 +++++------------ libs/bt/include/psemek/bt/process.hpp | 10 ++++++++- libs/bt/include/psemek/bt/status.hpp | 25 ++++++++++++++++++++++ libs/bt/include/psemek/bt/timeout.hpp | 18 ++++++++++++++-- libs/bt/include/psemek/bt/updater.hpp | 0 libs/bt/include/psemek/bt/wait.hpp | 16 +++++++++++++- 12 files changed, 148 insertions(+), 24 deletions(-) create mode 100644 libs/bt/include/psemek/bt/assert.hpp create mode 100644 libs/bt/include/psemek/bt/status.hpp delete mode 100644 libs/bt/include/psemek/bt/updater.hpp diff --git a/libs/bt/CMakeLists.txt b/libs/bt/CMakeLists.txt index 51cf68b0..f39a5379 100644 --- a/libs/bt/CMakeLists.txt +++ b/libs/bt/CMakeLists.txt @@ -3,4 +3,4 @@ file(GLOB_RECURSE PSEMEK_BT_SOURCES RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}" "sour psemek_add_library(psemek-bt ${PSEMEK_BT_HEADERS} ${PSEMEK_BT_SOURCES}) target_include_directories(psemek-bt PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") -target_link_libraries(psemek-bt PUBLIC psemek-util) +target_link_libraries(psemek-bt PUBLIC psemek-util psemek-log) diff --git a/libs/bt/include/psemek/bt/action.hpp b/libs/bt/include/psemek/bt/action.hpp index ff105bb3..2420f20f 100644 --- a/libs/bt/include/psemek/bt/action.hpp +++ b/libs/bt/include/psemek/bt/action.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace psemek::bt { @@ -25,7 +26,15 @@ namespace psemek::bt status update(Time dt, Args ... args) override { - action_fn_(dt, args...); + try + { + action_fn_(dt, args...); + } + catch (assertion_failed_exception const &) + { + return finished{false}; + } + return finished{true}; } diff --git a/libs/bt/include/psemek/bt/assert.hpp b/libs/bt/include/psemek/bt/assert.hpp new file mode 100644 index 00000000..a4cf5fc5 --- /dev/null +++ b/libs/bt/include/psemek/bt/assert.hpp @@ -0,0 +1,28 @@ +#pragma once + +#include +#include +#include +#include + +namespace psemek::bt +{ + + struct assertion_failed_exception + : util::exception + { + assertion_failed_exception(std::string message, boost::stacktrace::stacktrace stacktrace = {}) + : util::exception(std::move(message), std::move(stacktrace)) + {} + }; + + template + inline bool assertion_failed(Args const & ... args) + { + (::psemek::log::error() << ... << args); + throw assertion_failed_exception(util::to_string(args...)); + } + +} + +#define bt_assert(cond, ...) (void)(!(cond) && ::psemek::bt::assertion_failed("BT assertion failed: ", #cond, " ", ##__VA_ARGS__, " (", __FILE__, ":", __LINE__, ")")) diff --git a/libs/bt/include/psemek/bt/by_index.hpp b/libs/bt/include/psemek/bt/by_index.hpp index b217a0f5..88f8a185 100644 --- a/libs/bt/include/psemek/bt/by_index.hpp +++ b/libs/bt/include/psemek/bt/by_index.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include @@ -26,13 +27,27 @@ namespace psemek::bt void start(Args ... args) override { - current_index_ = index_fn_(args...); + failed_start_ = false; + + try + { + current_index_ = index_fn_(args...); + } + catch (assertion_failed_exception const &) + { + failed_start_ = true; + return; + } + if (current_index_ < children_.size()) children_[current_index_]->start(args...); } status update(Time dt, Args ... args) override { + if (failed_start_) + return finished{false}; + if (current_index_ >= children_.size()) return finished{false}; @@ -49,6 +64,7 @@ namespace psemek::bt private: IndexFn index_fn_; + bool failed_start_ = false; std::vector> children_; std::size_t current_index_ = 0; }; diff --git a/libs/bt/include/psemek/bt/condition.hpp b/libs/bt/include/psemek/bt/condition.hpp index f1003e6f..ce13a9c4 100644 --- a/libs/bt/include/psemek/bt/condition.hpp +++ b/libs/bt/include/psemek/bt/condition.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace psemek::bt { @@ -23,7 +24,14 @@ namespace psemek::bt status update(Time, Args ... args) override { - return finished{condition_fn_(args...)}; + try + { + return finished{condition_fn_(args...)}; + } + catch (assertion_failed_exception const &) + { + return finished{false}; + } } private: diff --git a/libs/bt/include/psemek/bt/lazy.hpp b/libs/bt/include/psemek/bt/lazy.hpp index e1a3cf7a..97a87092 100644 --- a/libs/bt/include/psemek/bt/lazy.hpp +++ b/libs/bt/include/psemek/bt/lazy.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include namespace psemek::bt { @@ -23,12 +24,22 @@ namespace psemek::bt void start(Args ... args) override { - child_ = node_factory_(args...); - child_->start(args...); + try + { + child_ = node_factory_(args...); + child_->start(args...); + } + catch (assertion_failed_exception const &) + { + child_ = nullptr; + } } status update(Time dt, Args ... args) override { + if (!child_) + return finished{false}; + auto result = child_->update(dt, args...); if (std::holds_alternative(result)) child_ = nullptr; diff --git a/libs/bt/include/psemek/bt/node.hpp b/libs/bt/include/psemek/bt/node.hpp index bceab9f3..7d7e790a 100644 --- a/libs/bt/include/psemek/bt/node.hpp +++ b/libs/bt/include/psemek/bt/node.hpp @@ -1,8 +1,8 @@ #pragma once #include +#include -#include #include namespace psemek::bt @@ -16,20 +16,11 @@ namespace psemek::bt { using tree_type = tree; - struct running - {}; + using running = detail::running; + using finished = detail::finished; + using suspended = detail::suspended