Skip to content

Commit ba88b83

Browse files
galouGaël Écorchard
andauthored
Honor pre- and post-conditions in ThreadedAction (#1286)
`ThreadedAction::executeTick()` overrides `TreeNode::executeTick()` without calling it, so the pre-conditions (`_skipIf`, `_failureIf`, `_successIf`, `_while`, `_onlyIf`, `_onStart`) and the post-conditions (`_onSuccess`, `_onFailure`, `_post`) were ignored on all subclasses of `ThreadedAction`: e.g. a node with `_skipIf="true"` was still ticked. Check them as `TreeNode::executeTick()` does. The pre- and post-tick callbacks are still not called for threaded actions. Signed-off-by: Gaël Écorchard <gael@km-robotics.cz> Co-authored-by: Gaël Écorchard <gael@km-robotics.cz>
1 parent ea74003 commit ba88b83

3 files changed

Lines changed: 94 additions & 8 deletions

File tree

‎include/behaviortree_cpp/tree_node.h‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -429,6 +429,7 @@ class TreeNode
429429
friend class BehaviorTreeFactory;
430430
friend class DecoratorNode;
431431
friend class ControlNode;
432+
friend class ThreadedAction;
432433
friend class Tree;
433434

434435
[[nodiscard]] NodeConfig& config();

‎src/action_node.cpp‎

Lines changed: 38 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -14,13 +14,16 @@
1414
#define MINICORO_IMPL
1515
#include "behaviortree_cpp/action_node.h"
1616

17+
#include "behaviortree_cpp/basic_types.h"
18+
1719
// minicoro.h includes this inside extern "C": include it first, at global scope.
1820
#ifdef __EMSCRIPTEN__
1921
#include <emscripten/fiber.h>
2022
#endif
2123

2224
#include "minicoro.h"
2325

26+
#include <exception>
2427
#include <iostream>
2528

2629
using namespace BT;
@@ -207,6 +210,25 @@ void StatefulActionNode::halt()
207210
NodeStatus BT::ThreadedAction::executeTick()
208211
{
209212
using lock_type = std::unique_lock<std::mutex>;
213+
214+
// This method overrides TreeNode::executeTick(), so it must handle the
215+
// pre-conditions (_skipIf, _failureIf, _successIf, _while) and the
216+
// post-conditions itself, as TreeNode::executeTick() does.
217+
if(auto precond = checkPreConditions())
218+
{
219+
const NodeStatus precond_status = precond.value();
220+
if(isStatusCompleted(precond_status))
221+
{
222+
checkPostConditions(precond_status);
223+
}
224+
// preserve the IDLE state if skipped, but communicate SKIPPED to parent
225+
if(precond_status != NodeStatus::SKIPPED)
226+
{
227+
setStatus(precond_status);
228+
}
229+
return precond_status;
230+
}
231+
210232
//send signal to other thread.
211233
// The other thread is in charge for changing the status
212234
if(status() == NodeStatus::IDLE)
@@ -236,16 +258,24 @@ NodeStatus BT::ThreadedAction::executeTick()
236258
});
237259
}
238260

239-
const lock_type l(mutex_);
240-
if(exptr_)
261+
NodeStatus new_status = NodeStatus::IDLE;
241262
{
242-
// The official interface of std::exception_ptr does not define any move
243-
// semantics. Thus, we copy and reset exptr_ manually.
244-
const auto exptr_copy = exptr_;
245-
exptr_ = nullptr;
246-
std::rethrow_exception(exptr_copy);
263+
const lock_type l(mutex_);
264+
if(exptr_)
265+
{
266+
// The official interface of std::exception_ptr does not define any move
267+
// semantics. Thus, we copy and reset exptr_ manually.
268+
const auto exptr_copy = exptr_;
269+
exptr_ = nullptr;
270+
std::rethrow_exception(exptr_copy);
271+
}
272+
new_status = status();
247273
}
248-
return status();
274+
if(isStatusCompleted(new_status))
275+
{
276+
checkPostConditions(new_status);
277+
}
278+
return new_status;
249279
}
250280

251281
void ThreadedAction::halt()

‎tests/gtest_skipping.cpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,12 @@
11
#include "action_test_node.h"
22
#include "test_helper.hpp"
33

4+
#include "behaviortree_cpp/action_node.h"
45
#include "behaviortree_cpp/basic_types.h"
56
#include "behaviortree_cpp/bt_factory.h"
7+
#include "behaviortree_cpp/tree_node.h"
68

9+
#include <atomic>
710
#include <string>
811

912
#include <gtest/gtest.h>
@@ -219,3 +222,55 @@ TEST(SkippingLogic, WhileSkip)
219222
// counters[1] contains the number of times TestB was ticked
220223
ASSERT_EQ(counters[1], 0);
221224
}
225+
226+
namespace
227+
{
228+
class ThreadedTickCounter : public ThreadedAction
229+
{
230+
public:
231+
ThreadedTickCounter(const std::string& name, const NodeConfig& config,
232+
std::atomic_int* ticks)
233+
: ThreadedAction(name, config), ticks_(ticks)
234+
{}
235+
236+
NodeStatus tick() override
237+
{
238+
++(*ticks_);
239+
return NodeStatus::SUCCESS;
240+
}
241+
242+
private:
243+
std::atomic_int* ticks_;
244+
};
245+
} // namespace
246+
247+
TEST(SkippingLogic, ThreadedActionPreAndPostConditions)
248+
{
249+
// ThreadedAction overrides executeTick(), it must still honor the
250+
// pre-conditions and run the post-conditions.
251+
BehaviorTreeFactory factory;
252+
std::atomic_int ticks = 0;
253+
factory.registerNodeType<ThreadedTickCounter>("Threaded", PortsList{}, &ticks);
254+
255+
const std::string xml_text = R"(
256+
<root BTCPP_format="4" >
257+
<BehaviorTree ID="MainTree">
258+
<Sequence>
259+
<Script code=" done:=0 "/>
260+
<Threaded _skipIf="true"/>
261+
<Threaded _successIf="true" _onSuccess="done:=done+1"/>
262+
<Threaded _onSuccess="done:=done+10"/>
263+
<Fallback>
264+
<Threaded _failureIf="true" _onFailure="done:=done+100"/>
265+
<AlwaysSuccess/>
266+
</Fallback>
267+
</Sequence>
268+
</BehaviorTree>
269+
</root>)";
270+
271+
auto tree = factory.createTreeFromText(xml_text);
272+
const auto status = tree.tickWhileRunning();
273+
ASSERT_EQ(status, NodeStatus::SUCCESS);
274+
ASSERT_EQ(ticks, 1);
275+
ASSERT_EQ(tree.rootBlackboard()->get<int>("done"), 111);
276+
}

0 commit comments

Comments
 (0)