BehaviorTree.CPP/src/loggers/bt_observer.cpp at 077a38b58aba749ec669e46fced4b2d7aaf116c2 · BehaviorTree/BehaviorTree.CPP · GitHub

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
118
119
120
#include "behaviortree_cpp/loggers/bt_observer.h"

#include "behaviortree_cpp/control_node.h"
#include "behaviortree_cpp/decorator_node.h"

namespace BT
{
TreeObserver::TreeObserver(const BT::Tree& tree) : StatusChangeLogger(tree.rootNode())
{
  std::function<void(const TreeNode&)> recursiveStep;

  recursiveStep = [&](const TreeNode& node) {
    if(auto control = dynamic_cast<const ControlNode*>(&node))
    {
      for(const auto& child : control->children())
      {
        recursiveStep(*child);
      }
    }
    else if(auto decorator = dynamic_cast<const DecoratorNode*>(&node))
    {
      if(decorator->type() != NodeType::SUBTREE)
      {
        recursiveStep(*decorator->child());
      }
    }

    if(_path_to_uid.count(node.fullPath()) != 0)
    {
      throw LogicError("TreeObserver not built correctly. Report issue");
    }
    _path_to_uid[node.fullPath()] = node.UID();
  };

  for(const auto& subtree : tree.subtrees)
  {
    recursiveStep(*subtree->nodes.front());
  }

  for(const auto& [path, uid] : _path_to_uid)
  {
    _statistics[uid] = {};
    _uid_to_path[uid] = path;
  }
}

TreeObserver::~TreeObserver()
{
  // Stop status callbacks before the statistics map is destroyed.
  unsubscribeFromTreeChanges();
}

void TreeObserver::callback(Duration timestamp, const TreeNode& node,
                            NodeStatus /*prev_status*/, NodeStatus status)
{
  auto& statistics = _statistics[node.UID()];
  statistics.current_status = status;
  statistics.last_timestamp = timestamp;

  if(status == NodeStatus::IDLE)
  {
    return;
  }

  statistics.transitions_count++;

  if(status == NodeStatus::SUCCESS)
  {
    statistics.last_result = status;
    statistics.success_count++;
  }
  else if(status == NodeStatus::FAILURE)
  {
    statistics.last_result = status;
    statistics.failure_count++;
  }
  else if(status == NodeStatus::SKIPPED)
  {
    statistics.skip_count++;
  }
}

const TreeObserver::NodeStatistics&
TreeObserver::getStatistics(const std::string& path) const
{
  auto it = _path_to_uid.find(path);
  if(it == _path_to_uid.end())
  {
    throw RuntimeError("TreeObserver::getStatistics: Invalid pattern");
  }
  return getStatistics(it->second);
}

const TreeObserver::NodeStatistics& TreeObserver::getStatistics(uint16_t uid) const
{
  auto it = _statistics.find(uid);
  if(it == _statistics.end())
  {
    throw RuntimeError("TreeObserver::getStatistics: Invalid UID");
  }
  return it->second;
}

const std::unordered_map<uint16_t, TreeObserver::NodeStatistics>&
TreeObserver::statistics() const
{
  return _statistics;
}

const std::unordered_map<std::string, uint16_t>& TreeObserver::pathToUID() const
{
  return _path_to_uid;
}

const std::map<uint16_t, std::string>& TreeObserver::uidToPath() const
{
  return _uid_to_path;
}

}  // namespace BT