Add bt_assert macro for non-crashing assertions in behavior trees

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)

libs/bt/include/psemek/bt/action.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 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};
 		}
 

libs/bt/include/psemek/bt/assert.hpp

@@ -0,0 +1,28 @@
+#pragma once
+
+#include <psemek/bt/status.hpp>
+#include <psemek/log/log.hpp>
+#include <psemek/util/exception.hpp>
+#include <psemek/util/to_string.hpp>
+
+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 <typename ... Args>
+	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__, ")"))

libs/bt/include/psemek/bt/by_index.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 #include <vector>
 
@@ -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<node_ptr<tree_type>> children_;
 		std::size_t current_index_ = 0;
 	};

libs/bt/include/psemek/bt/condition.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 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:

libs/bt/include/psemek/bt/lazy.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 namespace psemek::bt
 {
@@ -23,12 +24,22 @@ namespace psemek::bt
 
 		void start(Args ... args) override
 		{
-			child_ = node_factory_(args...);
+			try
-			child_->start(args...);
+			{
+				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<finished>(result))
 				child_ = nullptr;

libs/bt/include/psemek/bt/node.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
 #include <psemek/bt/tree.hpp>
+#include <psemek/bt/status.hpp>
 
-#include <variant>
 #include <memory>
 
 namespace psemek::bt
@@ -16,20 +16,11 @@ namespace psemek::bt
 	{
 		using tree_type = tree<Time, Event, Args...>;
 
-		struct running
+		using running = detail::running;
-		{};
+		using finished = detail::finished;
+		using suspended = detail::suspended<Time>;
 
-		struct finished
+		using status = detail::status<Time>;
-		{
-			bool result;
-		};
-
-		struct suspended
-		{
-			Time duration;
-		};
-
-		using status = std::variant<running, finished, suspended>;
 
 		virtual void start(Args ...) {}
 		virtual status update(Time dt, Args ... args) = 0;

libs/bt/include/psemek/bt/process.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 namespace psemek::bt
 {
@@ -26,7 +27,14 @@ namespace psemek::bt
 
 		status update(Time dt, Args ... args) override
 		{
-			return process_fn_(dt, args...);
+			try
+			{
+				return process_fn_(dt, args...);
+			}
+			catch (assertion_failed_exception const &)
+			{
+				return finished{false};
+			}
 		}
 
 		bool event(Event const &, Args ...) override

libs/bt/include/psemek/bt/status.hpp

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <variant>
+
+namespace psemek::bt::detail
+{
+
+	struct running
+	{};
+
+	struct finished
+	{
+		bool result;
+	};
+
+	template <typename Time>
+	struct suspended
+	{
+		Time duration;
+	};
+
+	template <typename Time>
+	using status = std::variant<running, finished, suspended<Time>>;
+
+}

libs/bt/include/psemek/bt/timeout.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 namespace psemek::bt
 {
@@ -24,12 +25,24 @@ namespace psemek::bt
 
 		void start(Args ... args) override
 		{
-			timer_ = duration_fn_(args...);
+			failed_start_ = false;
-			child_->start(args...);
+
+			try
+			{
+				timer_ = duration_fn_(args...);
+				child_->start(args...);
+			}
+			catch (assertion_failed_exception const &)
+			{
+				failed_start_ = true;
+			}
 		}
 
 		status update(Time dt, Args ... args) override
 		{
+			if (failed_start_)
+				return finished{false};
+
 			timer_ -= dt;
 			if (timer_ <= Time{0})
 				return finished{false};
@@ -46,6 +59,7 @@ namespace psemek::bt
 		node_ptr<tree<Time, Event, Args...>> child_;
 		DurationFn duration_fn_;
 		Time timer_ = Time{};
+		bool failed_start_ = false;
 	};
 
 	namespace detail

libs/bt/include/psemek/bt/wait.hpp

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <psemek/bt/node.hpp>
+#include <psemek/bt/assert.hpp>
 
 namespace psemek::bt
 {
@@ -24,11 +25,23 @@ namespace psemek::bt
 
 		void start(Args ... args) override
 		{
-			remaining_time_ = duration_fn_(args...);
+			failed_start_ = false;
+
+			try
+			{
+				remaining_time_ = duration_fn_(args...);
+			}
+			catch (assertion_failed_exception const &)
+			{
+				failed_start_ = true;
+			}
 		}
 
 		status update(Time dt, Args ...) override
 		{
+			if (failed_start_)
+				return finished{false};
+
 			remaining_time_ -= dt;
 			if (remaining_time_ <= 0)
 				return finished{true};
@@ -38,6 +51,7 @@ namespace psemek::bt
 	private:
 		DurationFn duration_fn_;
 		Time remaining_time_ = Time{};
+		bool failed_start_ = false;
 	};
 
 	namespace detail