Repository navigation
Expand file tree
/
Copy pathabstract_logger.cpp
More file actions
117 lines (97 loc) · 3.12 KB
/
Copy pathabstract_logger.cpp
File metadata and controls
117 lines (97 loc) · 3.12 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
#include "behaviortree_cpp/loggers/abstract_logger.h"
#include "behaviortree_cpp/utils/callback_gate.h"
#include <mutex>
namespace BT
{
struct StatusChangeLogger::PImpl
{
bool enabled = true;
bool show_transition_to_idle = true;
std::vector<TreeNode::StatusChangeSubscriber> subscribers;
TimestampType type = TimestampType::absolute;
BT::TimePoint first_timestamp = {};
std::mutex callback_mutex;
details::CallbackGate::Ptr callback_gate = std::make_shared<details::CallbackGate>();
};
StatusChangeLogger::StatusChangeLogger() : _p(std::make_unique<PImpl>())
{}
StatusChangeLogger::StatusChangeLogger(TreeNode* root_node) : StatusChangeLogger()
{
subscribeToTreeChanges(root_node);
}
StatusChangeLogger::~StatusChangeLogger()
{
unsubscribeFromTreeChanges();
}
void StatusChangeLogger::setEnabled(bool enabled)
{
const std::lock_guard lk(_p->callback_mutex);
_p->enabled = enabled;
}
void StatusChangeLogger::setTimestampType(TimestampType type)
{
const std::lock_guard lk(_p->callback_mutex);
_p->type = type;
}
bool StatusChangeLogger::enabled() const
{
const std::lock_guard lk(_p->callback_mutex);
return _p->enabled;
}
bool StatusChangeLogger::showsTransitionToIdle() const
{
const std::lock_guard lk(_p->callback_mutex);
return _p->show_transition_to_idle;
}
void StatusChangeLogger::enableTransitionToIdle(bool enable)
{
const std::lock_guard lk(_p->callback_mutex);
_p->show_transition_to_idle = enable;
}
void StatusChangeLogger::subscribeToTreeChanges(TreeNode* root_node)
{
_p->first_timestamp = std::chrono::high_resolution_clock::now();
auto subscribeCallback =
[this, gate = _p->callback_gate](TimePoint timestamp, const TreeNode& node,
NodeStatus prev, NodeStatus status) {
if(!gate->tryStart())
{
return;
}
const details::CallbackGate::Guard callback_guard(gate);
handleStatusChange(timestamp, node, prev, status);
};
auto visitor = [this, subscribeCallback](TreeNode* node) {
_p->subscribers.push_back(
node->subscribeToStatusChange(std::move(subscribeCallback)));
};
applyRecursiveVisitor(root_node, visitor);
}
void StatusChangeLogger::handleStatusChange(TimePoint timestamp, const TreeNode& node,
NodeStatus prev, NodeStatus status)
{
// Copy state under lock, then release before calling user code
// This prevents recursive mutex locking when multiple nodes change status
bool should_callback = false;
Duration adjusted_timestamp;
{
std::unique_lock lk(_p->callback_mutex);
if(_p->enabled && (status != NodeStatus::IDLE || _p->show_transition_to_idle))
{
should_callback = true;
adjusted_timestamp = (_p->type == TimestampType::absolute) ?
timestamp.time_since_epoch() :
(timestamp - _p->first_timestamp);
}
}
if(should_callback)
{
this->callback(adjusted_timestamp, node, prev, status);
}
}
void StatusChangeLogger::unsubscribeFromTreeChanges()
{
_p->callback_gate->closeAndDrain();
_p->subscribers.clear();
}
} // namespace BT