Skip to content

Commit 0ce11ef

Browse files
facontidavidefacontidavide
committed
WIP
1 parent 3cacf99 commit 0ce11ef

File tree

4 files changed

+216
-4
lines changed

4 files changed

+216
-4
lines changed

‎examples/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,7 @@ CompileExample("ex04_waypoints")
3939
CompileExample("ex05_subtree_model")
4040
CompileExample("ex06_access_by_ptr")
4141
CompileExample("ex07_blackboard_backup")
42+
CompileExample("ex08_sqlite_log")
4243

4344
CompileExample("t13_plugin_executor")
4445

‎examples/ex08_sqlite_log.cpp

+189
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,189 @@
1+
#include "dummy_nodes.h"
2+
#include "behaviortree_cpp/bt_factory.h"
3+
#include "behaviortree_cpp/loggers/bt_sqlite_logger.h"
4+
#include "behaviortree_cpp/xml_parsing.h"
5+
6+
#include "behaviortree_cpp/json_export.h"
7+
8+
struct TaskA
9+
{
10+
int type;
11+
std::string name;
12+
};
13+
14+
struct TaskB
15+
{
16+
double value;
17+
std::string name;
18+
};
19+
20+
using Command = std::variant<TaskA, TaskB>;
21+
22+
// Simple Action that updates an instance of Position2D in the blackboard
23+
class SetTask : public BT::SyncActionNode
24+
{
25+
public:
26+
SetTask(const std::string& name, const BT::NodeConfig& config)
27+
: BT::SyncActionNode(name, config)
28+
{}
29+
30+
BT::NodeStatus tick() override
31+
{
32+
auto type = getInput<std::string>("type").value();
33+
if(type == "A")
34+
{
35+
setOutput<Command>("task", TaskA{ 43, type });
36+
}
37+
else if(type == "B")
38+
{
39+
setOutput<Command>("task", TaskB{ 3.14, type });
40+
}
41+
return BT::NodeStatus::SUCCESS;
42+
}
43+
44+
static BT::PortsList providedPorts()
45+
{
46+
return { BT::InputPort<std::string>("type"), BT::OutputPort<Command>("task") };
47+
}
48+
49+
private:
50+
};
51+
52+
// clang-format off
53+
54+
static const char* xml_text = R"(
55+
<root BTCPP_format="4">
56+
57+
<BehaviorTree ID="MainTree">
58+
<Sequence>
59+
60+
<Script code="type:='A'" />
61+
<SetTask type="{type}" task="{task}" />
62+
<SubTree ID="ExecuteTaskA" task="{task}" _skipIf=" type!='A' " />
63+
64+
<Script code="type:='B'" />
65+
<SetTask type="{type}" task="{task}" />
66+
<SubTree ID="ExecuteTaskB" task="{task}" _skipIf=" type!='B' " />
67+
68+
</Sequence>
69+
</BehaviorTree>
70+
71+
<BehaviorTree ID="ExecuteTaskA">
72+
<Sequence>
73+
<Sleep msec="1000"/>
74+
<SaySomething message="executed command A" />
75+
</Sequence>
76+
</BehaviorTree>
77+
78+
<BehaviorTree ID="ExecuteTaskB">
79+
<Sequence>
80+
<Sleep msec="1000"/>
81+
<SaySomething message="executed command B" />
82+
</Sequence>
83+
</BehaviorTree>
84+
85+
</root>
86+
)";
87+
88+
// clang-format on
89+
90+
int main()
91+
{
92+
BT::BehaviorTreeFactory factory;
93+
94+
// Nodes registration, as usual
95+
factory.registerNodeType<DummyNodes::SaySomething>("SaySomething");
96+
factory.registerNodeType<SetTask>("SetTask");
97+
98+
// Groot2 editor requires a model of your registered Nodes.
99+
// You don't need to write that by hand, it can be automatically
100+
// generated using the following command.
101+
std::string xml_models = BT::writeTreeNodesModelXML(factory);
102+
103+
factory.registerBehaviorTreeFromText(xml_text);
104+
105+
auto tree = factory.createTree("MainTree");
106+
107+
std::cout << "----------- XML file ----------\n"
108+
<< BT::WriteTreeToXML(tree, false, false)
109+
<< "--------------------------------\n";
110+
111+
BT::SqliteLogger sqlite_logger(tree, "ex08_sqlitelog.db3", false);
112+
113+
//------------------------------------------------------------------------
114+
// Approach ONE: create additional tables.
115+
// This requires you to execute a "CREATE TABLE IF NOT EXISTS..." statement
116+
// at the beginning and then an "INSERT INTO TaskB VALUES .." statement
117+
// in a callback injected using sqlite_logger.setAdditionalCallback()
118+
119+
sqlite_logger.execSqlStatement("CREATE TABLE IF NOT EXISTS TaskA ("
120+
"name VARCHAR, "
121+
"type INTEGER );");
122+
123+
sqlite_logger.execSqlStatement("CREATE TABLE IF NOT EXISTS TaskB ("
124+
"name VARCHAR, "
125+
"value REAL );");
126+
127+
auto extra_callback = [&](BT::Duration timestamp, const BT::TreeNode& node,
128+
BT::NodeStatus prev_status, BT::NodeStatus status) -> void {
129+
if(prev_status == BT::NodeStatus::RUNNING && BT::isStatusCompleted(status))
130+
{
131+
if(node.name() == "ExecuteTaskA")
132+
{
133+
auto task = node.config().blackboard->get<Command>("task");
134+
auto taskA = std::get<TaskA>(task);
135+
auto str = BT::StrCat("INSERT INTO TaskA VALUES ", "('", taskA.name, "',",
136+
std::to_string(taskA.type), ");");
137+
sqlite_logger.execSqlStatement(str);
138+
}
139+
if(node.name() == "ExecuteTaskB")
140+
{
141+
auto task = node.config().blackboard->get<Command>("task");
142+
auto taskB = std::get<TaskB>(task);
143+
auto str = BT::StrCat("INSERT INTO TaskB VALUES ", "('", taskB.name, "',",
144+
std::to_string(taskB.value), ");");
145+
sqlite_logger.execSqlStatement(str);
146+
}
147+
}
148+
};
149+
sqlite_logger.setAdditionalCallback(extra_callback);
150+
151+
//------------------------------------------------------------------------
152+
// Approach TWO: serialize data into the extra column "metadata"
153+
// We will use JSON serialization
154+
155+
auto meta_callback = [&](BT::Duration timestamp, const BT::TreeNode& node,
156+
BT::NodeStatus prev_status,
157+
BT::NodeStatus status) -> std::string {
158+
if(prev_status == BT::NodeStatus::RUNNING && BT::isStatusCompleted(status))
159+
{
160+
if(node.name() == "ExecuteTaskA")
161+
{
162+
auto task = node.config().blackboard->get<Command>("task");
163+
auto taskA = std::get<TaskA>(task);
164+
nlohmann::json json;
165+
json["taskA"] = { { "name", taskA.name }, { "type", taskA.type } };
166+
return json.dump();
167+
}
168+
if(node.name() == "ExecuteTaskB")
169+
{
170+
auto task = node.config().blackboard->get<Command>("task");
171+
auto taskB = std::get<TaskB>(task);
172+
nlohmann::json json;
173+
json["taskB"] = { { "name", taskB.name }, { "value", taskB.value } };
174+
return json.dump();
175+
}
176+
}
177+
return {};
178+
};
179+
sqlite_logger.setMetadataCallback(meta_callback);
180+
//------------------------------------------------------------------------
181+
while(1)
182+
{
183+
std::cout << "Start" << std::endl;
184+
tree.tickWhileRunning();
185+
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
186+
}
187+
188+
return 0;
189+
}

‎include/behaviortree_cpp/loggers/bt_sqlite_logger.h

+10-3
Original file line numberDiff line numberDiff line change
@@ -39,13 +39,19 @@ class SqliteLogger : public StatusChangeLogger
3939

4040
// You can inject a function that add a metadata filed (a string) to the raw in the table.
4141
// The arguments of the function are the same as SqliteLogger::callback()
42-
using MetadataFunc =
42+
using MetadataCallback =
4343
std::function<std::string(Duration, const TreeNode&, NodeStatus, NodeStatus)>;
44-
void setMetadataCallback(MetadataFunc func);
44+
void setMetadataCallback(MetadataCallback func);
45+
46+
using ExtraCallback =
47+
std::function<void(Duration, const TreeNode&, NodeStatus, NodeStatus)>;
48+
void setAdditionalCallback(ExtraCallback func);
4549

4650
virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
4751
NodeStatus status) override;
4852

53+
void execSqlStatement(std::string statement);
54+
4955
virtual void flush() override;
5056

5157
private:
@@ -72,7 +78,8 @@ class SqliteLogger : public StatusChangeLogger
7278
std::thread writer_thread_;
7379
std::atomic_bool loop_ = true;
7480

75-
MetadataFunc meta_func_;
81+
MetadataCallback meta_func_;
82+
ExtraCallback extra_func_;
7683

7784
void writerLoop();
7885
};

‎src/loggers/bt_sqlite_logger.cpp

+16-1
Original file line numberDiff line numberDiff line change
@@ -77,11 +77,16 @@ SqliteLogger::~SqliteLogger()
7777
sqlite::Statement(*db_, "PRAGMA optimize;");
7878
}
7979

80-
void SqliteLogger::setMetadataCallback(MetadataFunc func)
80+
void SqliteLogger::setMetadataCallback(MetadataCallback func)
8181
{
8282
meta_func_ = func;
8383
}
8484

85+
void SqliteLogger::setAdditionalCallback(ExtraCallback func)
86+
{
87+
extra_func_ = func;
88+
}
89+
8590
void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
8691
NodeStatus prev_status, NodeStatus status)
8792
{
@@ -122,6 +127,16 @@ void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
122127
transitions_queue_.push_back(trans);
123128
}
124129
queue_cv_.notify_one();
130+
131+
if(extra_func_)
132+
{
133+
extra_func_(timestamp, node, prev_status, status);
134+
}
135+
}
136+
137+
void SqliteLogger::execSqlStatement(std::string statement)
138+
{
139+
sqlite::Statement(*db_, statement);
125140
}
126141

127142
void SqliteLogger::writerLoop()

0 commit comments

Comments
 (0)