Skip to content

Commit 8e9a77e

Browse files
facontidavidefacontidavide
committed
added Loop node
1 parent 224358b commit 8e9a77e

File tree

7 files changed

+189
-108
lines changed

7 files changed

+189
-108
lines changed

‎examples/ex04_waypoints.cpp

Lines changed: 33 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
#include "behaviortree_cpp/bt_factory.h"
2-
#include "behaviortree_cpp/actions/pop_from_queue.hpp"
3-
#include "behaviortree_cpp/decorators/consume_queue.h"
2+
#include "behaviortree_cpp/decorators/loop_node.h"
3+
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
44
#include <list>
55

66
using namespace BT;
@@ -30,68 +30,46 @@ class GenerateWaypoints : public SyncActionNode
3030

3131
NodeStatus tick() override
3232
{
33-
auto queue = std::make_shared<ProtectedQueue<Pose2D>>();
34-
for (int i = 0; i < 10; i++)
33+
auto shared_queue = std::make_shared<std::deque<Pose2D>>();
34+
for (int i = 0; i < 5; i++)
3535
{
36-
queue->items.push_back(Pose2D{double(i), double(i), 0});
36+
shared_queue->push_back(Pose2D{double(i), double(i), 0});
3737
}
38-
setOutput("waypoints", queue);
38+
setOutput("waypoints", shared_queue);
3939
return NodeStatus::SUCCESS;
4040
}
4141

4242
static PortsList providedPorts()
4343
{
44-
return {OutputPort<std::shared_ptr<ProtectedQueue<Pose2D>>>("waypoints")};
44+
return {OutputPort<SharedQueue<Pose2D>>("waypoints")};
4545
}
4646
};
4747
//--------------------------------------------------------------
48-
class UseWaypointQueue : public ThreadedAction
48+
class PrintNumber : public SyncActionNode
4949
{
50-
public:
51-
UseWaypointQueue(const std::string& name, const NodeConfig& config) :
52-
ThreadedAction(name, config)
50+
public:
51+
PrintNumber(const std::string& name, const NodeConfig& config) :
52+
SyncActionNode(name, config)
5353
{}
5454

5555
NodeStatus tick() override
5656
{
57-
std::shared_ptr<ProtectedQueue<Pose2D>> queue;
58-
if (getInput("waypoints", queue) && queue)
59-
{
60-
Pose2D wp;
61-
{
62-
// Since we are using reference semantic (the queue is wrapped in
63-
// a shared_ptr) to modify the queue inside the blackboard,
64-
// we are effectively bypassing the thread safety of the BB.
65-
// This is the reason why we need to use a mutex explicitly.
66-
std::unique_lock<std::mutex> lk(queue->mtx);
67-
68-
auto& waypoints = queue->items;
69-
if (waypoints.empty())
70-
{
71-
return NodeStatus::FAILURE;
72-
}
73-
wp = waypoints.front();
74-
waypoints.pop_front();
75-
76-
} // end mutex lock
77-
78-
std::this_thread::sleep_for(std::chrono::milliseconds(100));
79-
std::cout << "Using waypoint: " << wp.x << "/" << wp.y << std::endl;
80-
57+
double value;
58+
if (getInput("value", value)) {
59+
std::cout << "PrintNumber: " << value << "\n";
8160
return NodeStatus::SUCCESS;
8261
}
83-
else
84-
{
85-
return NodeStatus::FAILURE;
86-
}
62+
return NodeStatus::FAILURE;
8763
}
8864

8965
static PortsList providedPorts()
9066
{
91-
return {InputPort<std::shared_ptr<ProtectedQueue<Pose2D>>>("waypoints")};
67+
return {InputPort<double>("value")};
9268
}
9369
};
9470

71+
//--------------------------------------------------------------
72+
9573
/**
9674
* @brief Simple Action that uses the output of PopFromQueue<Pose2D> or ConsumeQueue<Pose2D>
9775
*/
@@ -124,46 +102,18 @@ class UseWaypoint : public ThreadedAction
124102
};
125103

126104
// clang-format off
127-
128-
static const char* xml_implicit = R"(
129-
<root BTCPP_format="4" >
130-
<BehaviorTree ID="TreeImplicit">
131-
<Sequence>
132-
<GenerateWaypoints waypoints="{waypoints}" />
133-
<KeepRunningUntilFailure>
134-
<UseWaypointQueue waypoints="{waypoints}" />
135-
</KeepRunningUntilFailure>
136-
</Sequence>
137-
</BehaviorTree>
138-
</root>
139-
)";
140-
141-
142-
static const char* xml_A = R"(
105+
static const char* xml_tree = R"(
143106
<root BTCPP_format="4" >
144107
<BehaviorTree ID="TreeA">
145108
<Sequence>
146-
<GenerateWaypoints waypoints="{waypoints}" />
147-
<QueueSize queue="{waypoints}" size="{wp_size}" />
148-
<Repeat num_cycles="{wp_size}" >
149-
<Sequence>
150-
<PopFromQueue queue="{waypoints}" popped_item="{wp}" />
151-
<UseWaypoint waypoint="{wp}" />
152-
</Sequence>
153-
</Repeat>
154-
</Sequence>
155-
</BehaviorTree>
156-
</root>
157-
)";
109+
<LoopPopDouble queue="1;2;3" value="{number}">
110+
<PrintNumber value="{number}" />
111+
</LoopPopDouble>
158112
159-
static const char* xml_B = R"(
160-
<root BTCPP_format="4" >
161-
<BehaviorTree ID="TreeB">
162-
<Sequence>
163113
<GenerateWaypoints waypoints="{waypoints}" />
164-
<ConsumeQueue queue="{waypoints}" popped_item="{wp}">
165-
<UseWaypoint waypoint="{wp}" />
166-
</ConsumeQueue>
114+
<LoopPopPose queue="{waypoints}" value="{wp}">
115+
<UseWaypoint waypoint="{wp}" />
116+
</LoopPopPose>
167117
</Sequence>
168118
</BehaviorTree>
169119
</root>
@@ -175,20 +125,18 @@ int main()
175125
{
176126
BehaviorTreeFactory factory;
177127

178-
factory.registerNodeType<PopFromQueue<Pose2D>>("PopFromQueue");
179-
factory.registerNodeType<QueueSize<Pose2D>>("QueueSize");
180-
factory.registerNodeType<ConsumeQueue<Pose2D>>("ConsumeQueue");
128+
factory.registerNodeType<LoopPopNode<Pose2D>>("LoopPopPose");
181129

182130
factory.registerNodeType<UseWaypoint>("UseWaypoint");
183-
factory.registerNodeType<UseWaypointQueue>("UseWaypointQueue");
131+
factory.registerNodeType<PrintNumber>("PrintNumber");
184132
factory.registerNodeType<GenerateWaypoints>("GenerateWaypoints");
185133

186-
for (const auto& xml_text : {xml_implicit, xml_A, xml_B})
187-
{
188-
auto tree = factory.createTreeFromText(xml_text);
189-
tree.tickWhileRunning();
190-
std::cout << "--------------" << std::endl;
191-
}
134+
auto tree = factory.createTreeFromText(xml_tree);
135+
136+
StdCoutLogger logger(tree);
137+
logger.enableTransitionToIdle(false);
138+
139+
tree.tickWhileRunning();
192140

193141
return 0;
194142
}

‎include/behaviortree_cpp/behavior_tree.h

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
#include "behaviortree_cpp/decorators/repeat_node.h"
3232
#include "behaviortree_cpp/decorators/run_once_node.h"
3333
#include "behaviortree_cpp/decorators/subtree_node.h"
34+
#include "behaviortree_cpp/decorators/loop_node.h"
3435

3536
#include "behaviortree_cpp/actions/always_success_node.h"
3637
#include "behaviortree_cpp/actions/always_failure_node.h"

‎include/behaviortree_cpp/decorators/loop_node.h

Lines changed: 132 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,132 @@
1+
/* Copyright (C) 2022 Davide Faconti - All Rights Reserved
2+
*
3+
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
4+
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
5+
* 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:
6+
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
7+
*
8+
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
9+
* 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,
10+
* 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.
11+
*/
12+
13+
#pragma once
14+
15+
#include <deque>
16+
#include <shared_mutex>
17+
#include "behaviortree_cpp/decorator_node.h"
18+
19+
namespace BT
20+
{
21+
22+
// this object will allow us to modify the queue in place,
23+
// when popping, in a thread safe-way and without copying the entire queue.
24+
template <typename T>
25+
using SharedQueue = std::shared_ptr<std::deque<T>>;
26+
27+
/**
28+
* @brief The LoopNode class is used to pop_front elements from a std::deque.
29+
* This element is copied into the port "value" and the child will be executed,
30+
* as long as we have elements in the queue.
31+
*
32+
* See Example 4: ex04_waypoints
33+
*
34+
* NOTE: unless T is `Any`, `string` or `double`, you must register the loop manually into
35+
* the factory.
36+
*/
37+
template <typename T = Any>
38+
class LoopPopNode : public DecoratorNode
39+
{
40+
bool child_running_ = false;
41+
SharedQueue<T> static_queue_;
42+
SharedQueue<T> current_queue_;
43+
std::string static_string_;
44+
45+
public:
46+
LoopPopNode(const std::string& name, const NodeConfig& config) :
47+
DecoratorNode(name, config)
48+
{
49+
auto raw_port = getRawPortValue("queue");
50+
if(!isBlackboardPointer(raw_port))
51+
{
52+
static_queue_ = convertFromString<SharedQueue<T>>(static_string_);
53+
}
54+
}
55+
56+
NodeStatus tick() override
57+
{
58+
bool popped = false;
59+
if(status() == NodeStatus::IDLE)
60+
{
61+
child_running_ = false;
62+
// special case: the port contains a string that was converted to SharedQueue<T>
63+
if(static_queue_)
64+
{
65+
current_queue_ = static_queue_;
66+
}
67+
}
68+
69+
// Pop value from queue, if the child is not RUNNING
70+
if(!child_running_)
71+
{
72+
// if the port is static, any_ref is empty, otherwise it will keep access to
73+
// port locked for thread-safety
74+
AnyWriteRef any_ref = static_queue_ ? AnyWriteRef() : getPortAny("queue");
75+
if(any_ref)
76+
{
77+
current_queue_ = any_ref.get()->cast<SharedQueue<T>>();
78+
}
79+
80+
if(current_queue_ && !current_queue_->empty()) {
81+
auto value = std::move(current_queue_->front());
82+
current_queue_->pop_front();
83+
popped = true;
84+
setOutput("value", value);
85+
}
86+
}
87+
88+
if(!popped && !child_running_)
89+
{
90+
return getInput<NodeStatus>("if_empty").value();
91+
}
92+
93+
if( status() == NodeStatus::IDLE)
94+
{
95+
setStatus(NodeStatus::RUNNING);
96+
}
97+
98+
NodeStatus child_state = child_node_->executeTick();
99+
child_running_ = (child_state == NodeStatus::RUNNING);
100+
101+
if(child_state == NodeStatus::FAILURE)
102+
{
103+
return NodeStatus::FAILURE;
104+
}
105+
return NodeStatus::RUNNING;
106+
}
107+
108+
static PortsList providedPorts()
109+
{
110+
// we mark "queue" as BidirectionalPort, because the original element is modified
111+
return {BidirectionalPort<SharedQueue<T>>("queue"),
112+
InputPort<NodeStatus>("if_empty", NodeStatus::SUCCESS,
113+
"Status to return if queue is empty: "
114+
"SUCCESS, FAILURE, SKIPPED"),
115+
OutputPort<T>("value")};
116+
}
117+
};
118+
119+
template <> inline
120+
SharedQueue<double> convertFromString<SharedQueue<double>>(StringView str)
121+
{
122+
auto parts = splitString(str, ';');
123+
SharedQueue<double> output = std::make_shared<std::deque<double>>();
124+
for (const StringView& part : parts)
125+
{
126+
output->push_back(convertFromString<double>(part));
127+
}
128+
return output;
129+
}
130+
131+
132+
} // namespace BT

‎include/behaviortree_cpp/tree_node.h

Lines changed: 9 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -255,16 +255,20 @@ class TreeNode
255255
*
256256
* What you must do, instead, to guaranty thread-safety, is:
257257
*
258-
* if(auto any_ref = getPortMutableAny("port_name")) {
259-
* auto foo_ptr = any_ref->cast<std::shared_ptr<Foo>>();
260-
* // modifying the content of foo_ptr __inside this scope__ IS thread-safe
258+
* if(auto any_ref = getPortAny("port_name")) {
259+
* Any* any = any_ref.get();
260+
* auto foo_ptr = any->cast<std::shared_ptr<Foo>>();
261+
* // modifying the content of foo_ptr inside this scope IS thread-safe
261262
* }
262263
*
263264
* It is important to destroy the object AnyWriteRef, to release the lock.
264265
*
266+
* NOTE: this method doesn't work if the port contains a static string, instead
267+
* of a blackboard pointer.
268+
*
265269
* @param key the identifier of the port.
266-
* @return empty AnyWriteRef is the port doesn't exist, reference to the content
267-
* of the port instead
270+
* @return empty AnyWriteRef if the blackboard entry doesn't exist or the content
271+
* of the port was a static string.
268272
*/
269273
AnyWriteRef getPortAny(const std::string& key);
270274

‎include/behaviortree_cpp/utils/locked_reference.hpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,7 @@ class LockedRef {
4646
const T* get() const{
4747
return ref_;
4848
}
49+
4950
T* get() {
5051
return ref_;
5152
}

‎src/bt_factory.cpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,9 @@ BehaviorTreeFactory::BehaviorTreeFactory()
7070
registerNodeType<SwitchNode<5>>("Switch5");
7171
registerNodeType<SwitchNode<6>>("Switch6");
7272

73+
registerNodeType<LoopPopNode<double>>("LoopPopDouble");
74+
registerNodeType<LoopPopNode<std::string>>("LoopPopString");
75+
7376
for (const auto& it : builders_)
7477
{
7578
builtin_IDs_.insert(it.first);

0 commit comments

Comments
 (0)