Skip to content

Commit e24f1dd

Browse files
facontidavidefacontidavide
committed
adding example 4 and queues
1 parent c14f25c commit e24f1dd

File tree

7 files changed

+463
-3
lines changed

7 files changed

+463
-3
lines changed

‎examples/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,3 +34,4 @@ endif()
3434
CompileExample("ex01_wrap_legacy")
3535
CompileExample("ex02_runtime_ports")
3636
CompileExample("ex03_ncurses_manual_selector")
37+
CompileExample("ex04_waypoints")

‎examples/ex04_waypoints.cpp

Lines changed: 201 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,201 @@
1+
#include "behaviortree_cpp_v3/bt_factory.h"
2+
#include "behaviortree_cpp_v3/actions/pop_from_queue.hpp"
3+
#include "behaviortree_cpp_v3/decorators/consume_queue.h"
4+
#include <list>
5+
6+
using namespace BT;
7+
8+
/*
9+
* In this example we will show how a common design pattern could be implemented.
10+
* We want to iterate through the elements of a queue, for instance a list of waypoints.
11+
*
12+
* Two ways to create a "loop" are presented, one using the actions "QueueSize" and "PopFromQueue"
13+
* and the other using the decorator "ConsumeQueue".
14+
*/
15+
16+
struct Pose2D
17+
{
18+
double x, y, theta;
19+
};
20+
21+
22+
/**
23+
* @brief Dummy action that generates a list of poses.
24+
*/
25+
class GenerateWaypoints : public SyncActionNode
26+
{
27+
public:
28+
GenerateWaypoints(const std::string& name, const NodeConfiguration& config)
29+
: SyncActionNode(name, config)
30+
{}
31+
32+
NodeStatus tick() override
33+
{
34+
auto queue = std::make_shared< ProtectedQueue<Pose2D> >();
35+
for(int i=0; i<10; i++)
36+
{
37+
queue->items.push_back( Pose2D{ double(i), double(i), 0} );
38+
}
39+
setOutput("waypoints", queue);
40+
return NodeStatus::SUCCESS;
41+
}
42+
43+
static PortsList providedPorts()
44+
{
45+
return { OutputPort< std::shared_ptr<ProtectedQueue<Pose2D> > >("waypoints") };
46+
}
47+
};
48+
//--------------------------------------------------------------
49+
class UseWaypointQueue : public AsyncActionNode
50+
{
51+
public:
52+
UseWaypointQueue(const std::string& name, const NodeConfiguration& config)
53+
: AsyncActionNode(name, config)
54+
{ }
55+
56+
NodeStatus tick() override
57+
{
58+
std::shared_ptr<ProtectedQueue<Pose2D>> queue;
59+
if( getInput("waypoints", queue) && queue )
60+
{
61+
Pose2D wp;
62+
{
63+
// Since we are using reference semantic (the queue is wrapped in
64+
// a shared_ptr) to modify the queue inside the blackboard,
65+
// we are effectively bypassing the thread safety of the BB.
66+
// This is the reason why we need to use a mutex explicitly.
67+
std::unique_lock<std::mutex> lk(queue->mtx);
68+
69+
auto& waypoints = queue->items;
70+
if( waypoints.empty() )
71+
{
72+
return NodeStatus::FAILURE;
73+
}
74+
wp = waypoints.front();
75+
waypoints.pop_front();
76+
77+
} // end mutex lock
78+
79+
std::this_thread::sleep_for( std::chrono::milliseconds(100) );
80+
std::cout << "Using waypoint: " << wp.x << "/" << wp.y << std::endl;
81+
82+
return NodeStatus::SUCCESS;
83+
}
84+
else{
85+
return NodeStatus::FAILURE;
86+
}
87+
}
88+
89+
static PortsList providedPorts()
90+
{
91+
return { InputPort< std::shared_ptr< ProtectedQueue<Pose2D>> >("waypoints") };
92+
}
93+
};
94+
95+
96+
/**
97+
* @brief Simple Action that uses the output of PopFromQueue<Pose2D> or ConsumeQueue<Pose2D>
98+
*/
99+
class UseWaypoint : public AsyncActionNode
100+
{
101+
public:
102+
UseWaypoint(const std::string& name, const NodeConfiguration& config)
103+
: AsyncActionNode(name, config)
104+
{ }
105+
106+
NodeStatus tick() override
107+
{
108+
Pose2D wp;
109+
if( getInput("waypoint", wp) )
110+
{
111+
std::this_thread::sleep_for( std::chrono::milliseconds(100) );
112+
std::cout << "Using waypoint: " << wp.x << "/" << wp.y << std::endl;
113+
return NodeStatus::SUCCESS;
114+
}
115+
else{
116+
return NodeStatus::FAILURE;
117+
}
118+
}
119+
120+
static PortsList providedPorts()
121+
{
122+
return { InputPort<Pose2D>("waypoint") };
123+
}
124+
};
125+
126+
127+
// clang-format off
128+
129+
static const char* xml_implicit = R"(
130+
<root main_tree_to_execute = "TreeImplicit" >
131+
<BehaviorTree ID="TreeImplicit">
132+
<Sequence>
133+
<GenerateWaypoints waypoints="{waypoints}" />
134+
<KeepRunningUntilFailure>
135+
<UseWaypointQueue waypoints="{waypoints}" />
136+
</KeepRunningUntilFailure>
137+
</Sequence>
138+
</BehaviorTree>
139+
</root>
140+
)";
141+
142+
143+
static const char* xml_A = R"(
144+
<root main_tree_to_execute = "TreeA" >
145+
<BehaviorTree ID="TreeA">
146+
<Sequence>
147+
<GenerateWaypoints waypoints="{waypoints}" />
148+
<QueueSize queue="{waypoints}" size="{wp_size}" />
149+
<Repeat num_cycles="{wp_size}" >
150+
<Sequence>
151+
<PopFromQueue queue="{waypoints}" popped_item="{wp}" />
152+
<UseWaypoint waypoint="{wp}" />
153+
</Sequence>
154+
</Repeat>
155+
</Sequence>
156+
</BehaviorTree>
157+
</root>
158+
)";
159+
160+
static const char* xml_B = R"(
161+
<root main_tree_to_execute = "TreeB" >
162+
<BehaviorTree ID="TreeB">
163+
<Sequence>
164+
<GenerateWaypoints waypoints="{waypoints}" />
165+
<ConsumeQueue queue="{waypoints}" popped_item="{wp}">
166+
<UseWaypoint waypoint="{wp}" />
167+
</ConsumeQueue>
168+
</Sequence>
169+
</BehaviorTree>
170+
</root>
171+
)";
172+
173+
// clang-format on
174+
175+
int main()
176+
{
177+
BehaviorTreeFactory factory;
178+
179+
factory.registerNodeType<PopFromQueue<Pose2D>>("PopFromQueue");
180+
factory.registerNodeType<QueueSize<Pose2D>>("QueueSize");
181+
factory.registerNodeType<ConsumeQueue<Pose2D>>("ConsumeQueue");
182+
183+
factory.registerNodeType<UseWaypoint>("UseWaypoint");
184+
factory.registerNodeType<UseWaypointQueue>("UseWaypointQueue");
185+
factory.registerNodeType<GenerateWaypoints>("GenerateWaypoints");
186+
187+
188+
for(const auto& xml_text : {xml_implicit, xml_A, xml_B} )
189+
{
190+
auto tree = factory.createTreeFromText(xml_text);
191+
while( tree.tickRoot() == NodeStatus::RUNNING )
192+
{
193+
tree.sleep( std::chrono::milliseconds(10) );
194+
}
195+
std::cout << "--------------" << std::endl;
196+
}
197+
198+
return 0;
199+
}
200+
201+

‎include/behaviortree_cpp_v3/actions/always_success_node.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
/* Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved
1+
/* Copyright (C) 2018-2022 Davide Faconti, Eurecat - All Rights Reserved
22
*
33
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
44
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,

‎include/behaviortree_cpp_v3/actions/pop_from_queue.hpp

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
/* Copyright (C) 2018-2022 Davide Faconti, Eurecat - 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+
14+
#ifndef BEHAVIORTREE_POPFROMQUEUE_HPP
15+
#define BEHAVIORTREE_POPFROMQUEUE_HPP
16+
17+
#include <list>
18+
#include <mutex>
19+
#include "behaviortree_cpp_v3/action_node.h"
20+
#include "behaviortree_cpp_v3/decorator_node.h"
21+
22+
23+
/**
24+
* Template Action used in ex04_waypoints.cpp example.
25+
*
26+
* Its purpose is to do make it easy to create while loops wich consume the elements of a queue.
27+
*
28+
* Note that modifying the queue is not thread safe, therefore the action that creates the queue
29+
* or push elements into it, must be Synchronous.
30+
*
31+
* When ticked, we pop_front from the "queue" and insert that value in "popped_item".
32+
* Return FAILURE if the queue is empty, SUCCESS otherwise.
33+
*/
34+
namespace BT
35+
{
36+
37+
template <typename T>
38+
struct ProtectedQueue
39+
{
40+
std::list<T> items;
41+
std::mutex mtx;
42+
};
43+
44+
/*
45+
* Few words about why we represent the queue as std::shared_ptr<ProtectedQueue>:
46+
*
47+
* Since we will pop from the queue, the fact that the blackboard uses
48+
* a value semantic is not very convenient, since it would oblige us to
49+
* copy the entire std::list from the BB and than copy again a new one with one less element.
50+
*
51+
* We avoid this using reference semantic (wrapping the object in a shared_ptr).
52+
* Unfortunately, remember that this makes our access to the list not thread-safe!
53+
* This is the reason why we add a mutex to be used when modyfying the ProtectedQueue::items
54+
*
55+
* */
56+
57+
58+
template <typename T>
59+
class PopFromQueue : public SyncActionNode
60+
{
61+
public:
62+
PopFromQueue(const std::string& name, const NodeConfiguration& config)
63+
: SyncActionNode(name, config)
64+
{
65+
}
66+
67+
NodeStatus tick() override
68+
{
69+
std::shared_ptr<ProtectedQueue<T>> queue;
70+
if( getInput("queue", queue) && queue )
71+
{
72+
std::unique_lock<std::mutex> lk(queue->mtx);
73+
auto& items = queue->items;
74+
75+
if( items.empty() )
76+
{
77+
return NodeStatus::FAILURE;
78+
}
79+
else{
80+
T val = items.front();
81+
items.pop_front();
82+
setOutput("popped_item", val);
83+
return NodeStatus::SUCCESS;
84+
}
85+
}
86+
else{
87+
return NodeStatus::FAILURE;
88+
}
89+
}
90+
91+
static PortsList providedPorts()
92+
{
93+
return { InputPort<std::shared_ptr<ProtectedQueue<T>>>("queue"),
94+
OutputPort<T>("popped_item")};
95+
}
96+
};
97+
98+
/**
99+
* Get the size of a queue. Usefull is you want to write something like:
100+
*
101+
* <QueueSize queue="{waypoints}" size="{wp_size}" />
102+
* <Repeat num_cycles="{wp_size}" >
103+
* <Sequence>
104+
* <PopFromQueue queue="{waypoints}" popped_item="{wp}" >
105+
* <UseWaypoint waypoint="{wp}" />
106+
* </Sequence>
107+
* </Repeat>
108+
*/
109+
template <typename T>
110+
class QueueSize : public SyncActionNode
111+
{
112+
public:
113+
QueueSize(const std::string& name, const NodeConfiguration& config)
114+
: SyncActionNode(name, config)
115+
{
116+
}
117+
118+
NodeStatus tick() override
119+
{
120+
std::shared_ptr<ProtectedQueue<T>> queue;
121+
if( getInput("queue", queue) && queue )
122+
{
123+
std::unique_lock<std::mutex> lk(queue->mtx);
124+
auto& items = queue->items;
125+
126+
if( items.empty() )
127+
{
128+
return NodeStatus::FAILURE;
129+
}
130+
else{
131+
setOutput("size", int(items.size()) );
132+
return NodeStatus::SUCCESS;
133+
}
134+
}
135+
return NodeStatus::FAILURE;
136+
}
137+
138+
static PortsList providedPorts()
139+
{
140+
return { InputPort<std::shared_ptr<ProtectedQueue<T>>>("queue"),
141+
OutputPort<int>("size")};
142+
}
143+
};
144+
145+
146+
}
147+
148+
#endif // BEHAVIORTREE_POPFROMQUEUE_HPP

‎include/behaviortree_cpp_v3/bt_factory.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -338,7 +338,8 @@ class BehaviorTreeFactory
338338
static_assert(default_constructable || param_constructable,
339339
"[registerNode]: the registered class must have at least one of these two "
340340
"constructors: "
341-
" (const std::string&, const NodeConfiguration&) or (const std::string&).");
341+
" (const std::string&, const NodeConfiguration&) or (const std::string&).\n"
342+
"Check also if the constructor is public!");
342343

343344
static_assert(!(param_constructable && !has_static_ports_list),
344345
"[registerNode]: you MUST implement the static method: "

0 commit comments

Comments
 (0)