Skip to content

Commit 3018e16

Browse files
facontidavidefacontidavide
committed
Merge branch 'log_snapshot'
2 parents 133c5fe + e5c256e commit 3018e16

File tree

5 files changed

+245
-6
lines changed

5 files changed

+245
-6
lines changed

‎examples/CMakeLists.txt

Lines changed: 1 addition & 0 deletions
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

Lines changed: 151 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,151 @@
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+
// Write some data (from the blackboard) and write into the
115+
// extra column called "extra_data". We will use JSON serialization
116+
117+
auto meta_callback = [&](BT::Duration timestamp, const BT::TreeNode& node,
118+
BT::NodeStatus prev_status,
119+
BT::NodeStatus status) -> std::string {
120+
if(prev_status == BT::NodeStatus::RUNNING && BT::isStatusCompleted(status))
121+
{
122+
if(node.name() == "ExecuteTaskA")
123+
{
124+
auto task = node.config().blackboard->get<Command>("task");
125+
auto taskA = std::get<TaskA>(task);
126+
nlohmann::json json;
127+
json["taskA"] = { { "name", taskA.name }, { "type", taskA.type } };
128+
return json.dump();
129+
}
130+
if(node.name() == "ExecuteTaskB")
131+
{
132+
auto task = node.config().blackboard->get<Command>("task");
133+
auto taskB = std::get<TaskB>(task);
134+
nlohmann::json json;
135+
json["taskB"] = { { "name", taskB.name }, { "value", taskB.value } };
136+
return json.dump();
137+
}
138+
}
139+
return {};
140+
};
141+
sqlite_logger.setAdditionalCallback(meta_callback);
142+
//------------------------------------------------------------------------
143+
while(1)
144+
{
145+
std::cout << "Start" << std::endl;
146+
tree.tickWhileRunning();
147+
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
148+
}
149+
150+
return 0;
151+
}

‎examples/t12_groot_howto.cpp

Lines changed: 16 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,7 @@ int main()
9898
// Groot2 editor requires a model of your registered Nodes.
9999
// You don't need to write that by hand, it can be automatically
100100
// generated using the following command.
101-
std::string xml_models = BT::writeTreeNodesModelXML(factory);
101+
const std::string xml_models = BT::writeTreeNodesModelXML(factory);
102102

103103
factory.registerBehaviorTreeFromText(xml_text);
104104

@@ -125,6 +125,21 @@ int main()
125125
bool append_to_database = true;
126126
BT::SqliteLogger sqlite_logger(tree, "t12_sqlitelog.db3", append_to_database);
127127

128+
// We can add some extra information to the SqliteLogger, for instance the value of the
129+
// "door_open" blackboard entry, at the end of node "tryOpen" (Fallback)
130+
131+
auto sqlite_callback = [](BT::Duration timestamp, const BT::TreeNode& node,
132+
BT::NodeStatus prev_status,
133+
BT::NodeStatus status) -> std::string {
134+
if(node.name() == "tryOpen" && BT::isStatusCompleted(status))
135+
{
136+
auto is_open = BT::toStr(node.config().blackboard->get<bool>("door_open"));
137+
return "[tryOpen] door_open=" + is_open;
138+
}
139+
return {};
140+
};
141+
sqlite_logger.setAdditionalCallback(sqlite_callback);
142+
128143
while(1)
129144
{
130145
std::cout << "Start" << std::endl;

‎include/behaviortree_cpp/loggers/bt_sqlite_logger.h

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,28 @@ class Connection;
1111
namespace BT
1212
{
1313

14+
/** SQL schema
15+
*
16+
* CREATE TABLE IF NOT EXISTS Definitions (
17+
* session_id INTEGER PRIMARY KEY AUTOINCREMENT,
18+
* date TEXT NOT NULL,
19+
* xml_tree TEXT NOT NULL);
20+
*
21+
* CREATE TABLE IF NOT EXISTS Nodes ("
22+
* session_id INTEGER NOT NULL,
23+
* fullpath VARCHAR, "
24+
* node_uid INTEGER NOT NULL );
25+
*
26+
* CREATE TABLE IF NOT EXISTS Transitions (
27+
* timestamp INTEGER PRIMARY KEY NOT NULL,
28+
* session_id INTEGER NOT NULL,
29+
* node_uid INTEGER NOT NULL,
30+
* duration INTEGER,
31+
* state INTEGER NOT NULL,
32+
* extra_data VARCHAR );
33+
*
34+
*/
35+
1436
/**
1537
* @brief The SqliteLogger is a logger that will store the tree and all the
1638
* status transitions in a SQLite database (single file).
@@ -37,9 +59,18 @@ class SqliteLogger : public StatusChangeLogger
3759

3860
virtual ~SqliteLogger() override;
3961

62+
// You can inject a function that add a string to the Transitions table,
63+
// in the column "extra_data".
64+
// The arguments of the function are the same as SqliteLogger::callback()
65+
using ExtraCallback =
66+
std::function<std::string(Duration, const TreeNode&, NodeStatus, NodeStatus)>;
67+
void setAdditionalCallback(ExtraCallback func);
68+
4069
virtual void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
4170
NodeStatus status) override;
4271

72+
void execSqlStatement(std::string statement);
73+
4374
virtual void flush() override;
4475

4576
private:
@@ -56,6 +87,7 @@ class SqliteLogger : public StatusChangeLogger
5687
int64_t timestamp;
5788
int64_t duration;
5889
NodeStatus status;
90+
std::string extra_data;
5991
};
6092

6193
std::deque<Transition> transitions_queue_;
@@ -65,6 +97,8 @@ class SqliteLogger : public StatusChangeLogger
6597
std::thread writer_thread_;
6698
std::atomic_bool loop_ = true;
6799

100+
ExtraCallback extra_func_;
101+
68102
void writerLoop();
69103
};
70104

‎src/loggers/bt_sqlite_logger.cpp

Lines changed: 43 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,15 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
2222
sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Transitions ("
2323
"timestamp INTEGER PRIMARY KEY NOT NULL, "
2424
"session_id INTEGER NOT NULL, "
25-
"uid INTEGER NOT NULL, "
25+
"node_uid INTEGER NOT NULL, "
2626
"duration INTEGER, "
27-
"state INTEGER NOT NULL);");
27+
"state INTEGER NOT NULL,"
28+
"extra_data VARCHAR );");
29+
30+
sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Nodes ("
31+
"session_id INTEGER NOT NULL, "
32+
"fullpath VARCHAR, "
33+
"node_uid INTEGER NOT NULL );");
2834

2935
sqlite::Statement(*db_, "CREATE TABLE IF NOT EXISTS Definitions ("
3036
"session_id INTEGER PRIMARY KEY AUTOINCREMENT, "
@@ -35,6 +41,7 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
3541
{
3642
sqlite::Statement(*db_, "DELETE from Transitions;");
3743
sqlite::Statement(*db_, "DELETE from Definitions;");
44+
sqlite::Statement(*db_, "DELETE from Nodes;");
3845
}
3946

4047
auto tree_xml = WriteTreeToXML(tree, true, true);
@@ -43,12 +50,23 @@ SqliteLogger::SqliteLogger(const Tree& tree, std::filesystem::path const& filepa
4350
"VALUES (datetime('now','localtime'),?);",
4451
tree_xml);
4552

46-
auto res = sqlite::Query(*db_, "SELECT MAX(session_id) FROM Definitions LIMIT 1;");
53+
auto res = sqlite::Query(*db_, "SELECT MAX(session_id) "
54+
"FROM Definitions LIMIT 1;");
4755

4856
while(res.Next())
4957
{
5058
session_id_ = res.Get(0);
5159
}
60+
61+
for(const auto& subtree : tree.subtrees)
62+
{
63+
for(const auto& node : subtree->nodes)
64+
{
65+
sqlite::Statement(*db_, "INSERT INTO Nodes VALUES (?, ?, ?)", session_id_,
66+
node->fullPath(), node->UID());
67+
}
68+
}
69+
5270
writer_thread_ = std::thread(&SqliteLogger::writerLoop, this);
5371
}
5472

@@ -61,6 +79,11 @@ SqliteLogger::~SqliteLogger()
6179
sqlite::Statement(*db_, "PRAGMA optimize;");
6280
}
6381

82+
void SqliteLogger::setAdditionalCallback(ExtraCallback func)
83+
{
84+
extra_func_ = func;
85+
}
86+
6487
void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
6588
NodeStatus prev_status, NodeStatus status)
6689
{
@@ -91,11 +114,26 @@ void SqliteLogger::callback(Duration timestamp, const TreeNode& node,
91114
trans.node_uid = node.UID();
92115
trans.status = status;
93116

117+
if(extra_func_)
118+
{
119+
trans.extra_data = extra_func_(timestamp, node, prev_status, status);
120+
}
121+
94122
{
95123
std::scoped_lock lk(queue_mutex_);
96124
transitions_queue_.push_back(trans);
97125
}
98126
queue_cv_.notify_one();
127+
128+
if(extra_func_)
129+
{
130+
extra_func_(timestamp, node, prev_status, status);
131+
}
132+
}
133+
134+
void SqliteLogger::execSqlStatement(std::string statement)
135+
{
136+
sqlite::Statement(*db_, statement);
99137
}
100138

101139
void SqliteLogger::writerLoop()
@@ -116,9 +154,9 @@ void SqliteLogger::writerLoop()
116154
auto const trans = transitions.front();
117155
transitions.pop_front();
118156

119-
sqlite::Statement(*db_, "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
157+
sqlite::Statement(*db_, "INSERT INTO Transitions VALUES (?, ?, ?, ?, ?, ?)",
120158
trans.timestamp, session_id_, trans.node_uid, trans.duration,
121-
static_cast<int>(trans.status));
159+
static_cast<int>(trans.status), trans.extra_data);
122160
}
123161
}
124162
}

0 commit comments

Comments
 (0)