Thanks to visit codestin.com
Credit goes to github.com

Skip to content

Commit 03980cc

Browse files
facontidavidefacontidavide
committed
add FileLogger2
1 parent 222384b commit 03980cc

File tree

8 files changed

+208
-43
lines changed

8 files changed

+208
-43
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -119,7 +119,7 @@ list(APPEND BT_SOURCE
119119
src/controls/while_do_else_node.cpp
120120

121121
src/loggers/bt_cout_logger.cpp
122-
src/loggers/bt_file_logger.cpp
122+
src/loggers/bt_file_logger_v2.cpp
123123
src/loggers/bt_minitrace_logger.cpp
124124
src/loggers/bt_observer.cpp
125125

‎examples/t12_groot_howto.cpp

Lines changed: 12 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
12
#include "crossdoor_nodes.h"
23
#include "behaviortree_cpp/bt_factory.h"
34
#include "behaviortree_cpp/loggers/groot2_publisher.h"
@@ -56,6 +57,7 @@ int main()
5657
factory.registerBehaviorTreeFromText(xml_text);
5758
auto tree = factory.createTree("MainTree");
5859

60+
5961
std::cout << "----------- XML file ----------\n"
6062
<< BT::WriteTreeToXML(tree, false)
6163
<< "--------------------------------\n";
@@ -64,12 +66,21 @@ int main()
6466
// get the tree and poll status updates.
6567
BT::Groot2Publisher publisher(tree);
6668

69+
// Add also two logger which save the transitions into a file.
70+
// Both formats are compatible with Groot2
71+
72+
// Lightweight serialization
73+
BT::FileLogger2 logger2(tree, "t12_logger2.btlog");
74+
// SQLite logger can save multiple sessions into the same database
75+
bool append_to_database = true;
76+
BT::SqliteLogger sqlite_logger(tree, "t12_logger2.btsql", append_to_database);
77+
6778
while(1)
6879
{
6980
std::cout << "Start" << std::endl;
7081
cross_door.reset();
7182
tree.tickWhileRunning();
72-
std::this_thread::sleep_for(std::chrono::milliseconds(3000));
83+
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
7384
}
7485

7586
return 0;

‎include/behaviortree_cpp/flatbuffers/bt_flatbuffer_helper.h

Lines changed: 2 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,7 @@
1-
#ifndef BT_FLATBUFFER_HELPER_H
2-
#define BT_FLATBUFFER_HELPER_H
1+
#pragma once
32

43
#include "behaviortree_cpp/bt_factory.h"
5-
#include "BT_logger_generated.h"
4+
#include "behaviortree_cpp/flatbuffers/BT_logger_generated.h"
65

76
namespace BT
87
{
@@ -160,4 +159,3 @@ inline SerializedTransition SerializeTransition(uint16_t UID, Duration timestamp
160159

161160
} // namespace BT
162161

163-
#endif // BT_FLATBUFFER_HELPER_H

‎include/behaviortree_cpp/loggers/bt_file_logger.h

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,9 @@
88

99
namespace BT
1010
{
11-
class FileLogger : public StatusChangeLogger
11+
12+
class [[deprecated("Use FileLogger2 instead")]]
13+
FileLogger : public StatusChangeLogger
1214
{
1315
public:
1416
FileLogger(const Tree& tree, const char* filename, uint16_t buffer_size = 10);

‎include/behaviortree_cpp/loggers/bt_file_logger_v2.h

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
#pragma once
2+
#include <fstream>
3+
#include <deque>
4+
#include <array>
5+
#include <filesystem>
6+
#include "behaviortree_cpp/loggers/abstract_logger.h"
7+
8+
namespace BT
9+
{
10+
/**
11+
* @brief The FileLogger2 is a logger that saves the tree as
12+
* XML and all the transitions. Data is written to file in
13+
* a separate thread, to minimize latency.
14+
*
15+
* Format:
16+
*
17+
* - first 4 bytes: size of the XML string (N)
18+
* - next N bytes: string containing the XML representing the tree.
19+
* - next 8 bytes: first timestamp (microseconds since epoch)
20+
* - next: each 8 bytes is a FileLogger2::Transition. See definition.
21+
*
22+
*/
23+
class FileLogger2 : public StatusChangeLogger
24+
{
25+
public:
26+
FileLogger2(const Tree& tree, std::filesystem::path const& filepath);
27+
28+
virtual ~FileLogger2() override;
29+
30+
void callback(Duration timestamp, const TreeNode& node, NodeStatus prev_status,
31+
NodeStatus status) override;
32+
33+
struct Transition
34+
{
35+
// 45 bits are enough for 35 million seconds (more than 1 year) relative to the first timestamp
36+
uint64_t timestamp_usec : 45;
37+
// if you have more than 64.000 nodes, you are doing something wrong :)
38+
uint64_t node_uid : 16;
39+
// enough bits to contain NodeStatus
40+
uint64_t status : 3;
41+
};
42+
43+
void flush() override;
44+
45+
private:
46+
std::ofstream file_stream_;
47+
48+
Duration first_timestamp_ = {};
49+
50+
std::deque<Transition> transitions_queue_;
51+
std::condition_variable queue_cv_;
52+
std::mutex queue_mutex_;
53+
54+
std::thread writer_thread_;
55+
std::atomic_bool loop_ = true;
56+
57+
void writerLoop();
58+
};
59+
60+
} // namespace BT
61+

‎include/behaviortree_cpp/loggers/bt_sqlite_logger.h

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -52,15 +52,14 @@ class SqliteLogger : public StatusChangeLogger
5252
NodeStatus status;
5353
};
5454

55-
std::deque<Transition> write_queue_;
56-
std::condition_variable queue_push_cv_;
57-
std::condition_variable queue_pop_cv_;
55+
std::deque<Transition> transitions_queue_;
56+
std::condition_variable queue_cv_;
5857
std::mutex queue_mutex_;
5958

60-
std::thread queue_thread_;
59+
std::thread writer_thread_;
6160
std::atomic_bool loop_ = true;
6261

63-
void threadLoop();
62+
void writerLoop();
6463

6564
};
6665

‎src/loggers/bt_file_logger_v2.cpp

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
2+
#include "behaviortree_cpp/flatbuffers/base.h"
3+
#include "behaviortree_cpp/xml_parsing.h"
4+
5+
namespace BT
6+
{
7+
8+
int64_t ToUsec(Duration ts)
9+
{
10+
return std::chrono::duration_cast<std::chrono::microseconds>(ts).count();
11+
}
12+
13+
FileLogger2::FileLogger2(const BT::Tree& tree, std::filesystem::path const& filepath) :
14+
StatusChangeLogger(tree.rootNode())
15+
{
16+
enableTransitionToIdle(true);
17+
18+
//-------------------------------------
19+
file_stream_.open(filepath, std::ofstream::binary | std::ofstream::out);
20+
21+
std::string const xml = WriteTreeToXML(tree, true);
22+
23+
// serialize the length of the buffer in the first 4 bytes
24+
char write_buffer[8];
25+
flatbuffers::WriteScalar(write_buffer, static_cast<int32_t>(xml.size()));
26+
file_stream_.write(write_buffer, 4);
27+
28+
// write the XML definition
29+
file_stream_.write(xml.data(), int(xml.size()));
30+
31+
first_timestamp_ = std::chrono::system_clock::now().time_since_epoch();
32+
33+
// save the first timestamp in the next 8 bytes (microseconds)
34+
int64_t timestamp_usec = ToUsec(first_timestamp_);
35+
flatbuffers::WriteScalar(write_buffer, timestamp_usec);
36+
file_stream_.write(write_buffer, 8);
37+
38+
writer_thread_ = std::thread(&FileLogger2::writerLoop, this);
39+
}
40+
41+
FileLogger2::~FileLogger2()
42+
{
43+
loop_ = false;
44+
queue_cv_.notify_one();
45+
writer_thread_.join();
46+
file_stream_.close();
47+
}
48+
49+
void FileLogger2::callback(Duration timestamp, const TreeNode& node,
50+
NodeStatus /*prev_status*/, NodeStatus status)
51+
{
52+
Transition trans;
53+
trans.timestamp_usec = uint64_t(ToUsec(timestamp - first_timestamp_));
54+
trans.node_uid = node.UID();
55+
trans.status = static_cast<uint64_t>(status);
56+
{
57+
std::scoped_lock lock(queue_mutex_);
58+
transitions_queue_.push_back(trans);
59+
}
60+
queue_cv_.notify_one();
61+
}
62+
63+
void FileLogger2::flush()
64+
{
65+
file_stream_.flush();
66+
}
67+
68+
void FileLogger2::writerLoop()
69+
{
70+
// local buffer in this thread
71+
std::deque<Transition> transitions;
72+
73+
while(loop_)
74+
{
75+
transitions.clear();
76+
{
77+
std::unique_lock lock(queue_mutex_);
78+
queue_cv_.wait_for(lock, std::chrono::milliseconds(10),
79+
[this]() {return !transitions_queue_.empty() && loop_; } );
80+
// simple way to pop all the transitions from transitions_queue_ into transitions
81+
std::swap(transitions, transitions_queue_);
82+
}
83+
while(!transitions.empty())
84+
{
85+
char write_buffer[8];
86+
flatbuffers::WriteScalar(write_buffer, transitions.front());
87+
file_stream_.write(write_buffer, 8);
88+
transitions.pop_front();
89+
}
90+
file_stream_.flush();
91+
}
92+
}
93+
94+
} // namespace BT

‎src/loggers/bt_sqlite_logger.cpp

Lines changed: 31 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -49,15 +49,15 @@ SqliteLogger::SqliteLogger(const Tree &tree,
4949
{
5050
session_id_ = res.Get(0);
5151
}
52-
queue_thread_ = std::thread(&SqliteLogger::threadLoop, this);
52+
writer_thread_ = std::thread(&SqliteLogger::writerLoop, this);
5353
}
5454

5555
SqliteLogger::~SqliteLogger()
5656
{
57-
flush();
5857
loop_ = false;
59-
queue_push_cv_.notify_one();
60-
queue_thread_.join();
58+
queue_cv_.notify_one();
59+
writer_thread_.join();
60+
flush();
6161
sqlite::Statement(*db_, "PRAGMA optimize;");
6262
}
6363

@@ -96,46 +96,46 @@ void SqliteLogger::callback(Duration timestamp,
9696

9797
{
9898
std::scoped_lock lk(queue_mutex_);
99-
write_queue_.push_back(trans);
99+
transitions_queue_.push_back(trans);
100100
}
101-
queue_push_cv_.notify_one();
101+
queue_cv_.notify_one();
102102
}
103103

104-
void SqliteLogger::threadLoop()
104+
void SqliteLogger::writerLoop()
105105
{
106+
std::deque<Transition> transitions;
107+
106108
while(loop_)
107109
{
108-
std::unique_lock lk(queue_mutex_);
109-
queue_push_cv_.wait(lk, [this]() {
110-
return !write_queue_.empty() || !loop_;
111-
});
112-
113-
if(!loop_) {
114-
break;
110+
transitions.clear();
111+
{
112+
std::unique_lock lk(queue_mutex_);
113+
queue_cv_.wait(lk, [this]() {
114+
return !transitions_queue_.empty() || !loop_;
115+
});
116+
std::swap(transitions, transitions_queue_);
115117
}
116118

117-
auto const trans = write_queue_.front();
118-
write_queue_.pop_front();
119-
lk.unlock();
120-
queue_pop_cv_.notify_all();
121-
122-
sqlite::Statement(
123-
*db_,
124-
"INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
125-
trans.timestamp,
126-
session_id_,
127-
trans.node_uid,
128-
trans.duration,
129-
static_cast<int>(trans.status));
119+
while(!transitions.empty())
120+
{
121+
auto const trans = transitions.front();
122+
transitions.pop_front();
123+
124+
sqlite::Statement(
125+
*db_,
126+
"INSERT INTO Transitions VALUES (?, ?, ?, ?, ?)",
127+
trans.timestamp,
128+
session_id_,
129+
trans.node_uid,
130+
trans.duration,
131+
static_cast<int>(trans.status));
132+
}
130133
}
131134
}
132135

133136
void BT::SqliteLogger::flush()
134137
{
135-
std::unique_lock lk(queue_mutex_);
136-
queue_pop_cv_.wait(lk, [&]() {
137-
return write_queue_.empty();
138-
});
138+
sqlite3_db_cacheflush(db_->GetPtr());
139139
}
140140

141141
}

0 commit comments

Comments
 (0)