Repository navigation
Expand file tree
/
Copy pathfinally_node.cpp
More file actions
143 lines (130 loc) · 3.1 KB
/
Copy pathfinally_node.cpp
File metadata and controls
143 lines (130 loc) · 3.1 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
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#include "behaviortree_cpp/controls/finally_node.h"
#include <exception>
#include <iostream>
#include <string_view>
#include <utility>
namespace BT
{
namespace
{
void printException(std::string_view node_name, const char* context,
const std::exception_ptr& exception)
{
std::cerr << "[" << node_name << "]: " << context << ": ";
try
{
std::rethrow_exception(exception);
}
catch(const std::exception& ex)
{
std::cerr << ex.what() << std::endl;
}
catch(...)
{
std::cerr << "non-std exception" << std::endl;
}
}
} // namespace
FinallyNode::FinallyNode(const std::string& name, const NodeConfig& config)
: ControlNode::ControlNode(name, config)
{
setRegistrationID("Finally");
}
void FinallyNode::halt()
{
// halt() also runs from ~Tree(), where a propagating exception terminates, so nothing here throws.
if(!in_cleanup_ && status() == NodeStatus::RUNNING && children_nodes_.size() == 2)
{
haltChildNoThrow(0);
try
{
if(children_nodes_[1]->executeTick() == NodeStatus::FAILURE)
{
std::cerr << "[" << name() << "]: cleanup returned FAILURE during halt"
<< std::endl;
}
}
catch(...)
{
printException(name(), "cleanup threw during halt", std::current_exception());
}
}
for(size_t i = 0; i < children_nodes_.size(); i++)
{
haltChildNoThrow(i);
}
in_cleanup_ = false;
main_status_ = NodeStatus::IDLE;
main_exception_ = nullptr;
resetStatus();
}
void FinallyNode::haltChildNoThrow(size_t i)
{
try
{
haltChild(i);
}
catch(...)
{
printException(name(), "a child threw while being halted", std::current_exception());
}
}
NodeStatus FinallyNode::tick()
{
if(children_nodes_.size() != 2)
{
throw LogicError("[", name(), "]: Finally requires exactly 2 children");
}
if(!isStatusActive(status()))
{
in_cleanup_ = false;
main_exception_ = nullptr;
}
setStatus(NodeStatus::RUNNING);
if(!in_cleanup_)
{
try
{
main_status_ = children_nodes_[0]->executeTick();
}
catch(...)
{
main_exception_ = std::current_exception();
haltChildNoThrow(0);
main_status_ = NodeStatus::FAILURE;
}
if(main_status_ == NodeStatus::RUNNING)
{
return NodeStatus::RUNNING;
}
if(main_status_ == NodeStatus::IDLE)
{
throw LogicError("[", name(), "]: A child should not return IDLE");
}
in_cleanup_ = true;
}
const NodeStatus cleanup_status = children_nodes_[1]->executeTick();
if(cleanup_status == NodeStatus::RUNNING)
{
return NodeStatus::RUNNING;
}
resetChildren();
in_cleanup_ = false;
if(main_exception_)
{
// executeTick() keeps our RUNNING status when tick() throws, and halt() would rerun cleanup.
resetStatus();
std::rethrow_exception(std::exchange(main_exception_, nullptr));
}
if(cleanup_status == NodeStatus::FAILURE)
{
return NodeStatus::FAILURE;
}
if(main_status_ == NodeStatus::SKIPPED)
{
// executeTick() keeps our RUNNING status on SKIPPED, and halt() would rerun cleanup.
resetStatus();
}
return main_status_;
}
} // namespace BT