-
Notifications
You must be signed in to change notification settings - Fork 857
Expand file tree
/
Copy pathretry_node.cpp
More file actions
105 lines (89 loc) · 3.34 KB
/
Copy pathretry_node.cpp
File metadata and controls
105 lines (89 loc) · 3.34 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
/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved
* Copyright (C) 2018-2025 Davide Faconti, Eurecat - All Rights Reserved
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
* The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
* WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
#include "behaviortree_cpp/decorators/retry_node.h"
namespace BT
{
RetryNode::RetryNode(const std::string& name, int NTries)
: DecoratorNode(name, {})
, max_attempts_(NTries)
, try_count_(0)
, read_parameter_from_ports_(false)
{
setRegistrationID("RetryUntilSuccessful");
}
RetryNode::RetryNode(const std::string& name, const NodeConfig& config)
: DecoratorNode(name, config)
, max_attempts_(0)
, try_count_(0)
, read_parameter_from_ports_(true)
{}
void RetryNode::halt()
{
try_count_ = 0;
DecoratorNode::halt();
}
NodeStatus RetryNode::tick()
{
if(read_parameter_from_ports_)
{
if(!getInput(NUM_ATTEMPTS, max_attempts_))
{
throw RuntimeError("Missing parameter [", NUM_ATTEMPTS, "] in RetryNode");
}
}
bool do_loop = try_count_ < max_attempts_ || max_attempts_ == -1;
setStatus(NodeStatus::RUNNING);
while(do_loop)
{
const NodeStatus prev_status = child_node_->status();
const NodeStatus child_status = child_node_->executeTick();
switch(child_status)
{
case NodeStatus::SUCCESS: {
try_count_ = 0;
resetChild();
return (NodeStatus::SUCCESS);
}
case NodeStatus::FAILURE: {
try_count_++;
// Refresh max_attempts_ in case it changed in one of the child nodes
getInput(NUM_ATTEMPTS, max_attempts_);
do_loop = try_count_ < max_attempts_ || max_attempts_ == -1;
resetChild();
// Return the execution flow if the child is async,
// to make this interruptible.
if(requiresWakeUp() && prev_status == NodeStatus::IDLE && do_loop)
{
emitWakeUpSignal();
return NodeStatus::RUNNING;
}
}
break;
case NodeStatus::RUNNING: {
return NodeStatus::RUNNING;
}
case NodeStatus::SKIPPED: {
// to allow it to be skipped again, we must reset the node
resetChild();
// the child has been skipped. Slip this too
return NodeStatus::SKIPPED;
}
case NodeStatus::IDLE: {
throw LogicError("[", name(), "]: A children should not return IDLE");
}
}
}
try_count_ = 0;
return NodeStatus::FAILURE;
}
} // namespace BT