parallel_robot.cpp
Go to the documentation of this file.
1 #include "bt/parallel_robot.h"
2 
3 /* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved
4  * Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved
5 *
6 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"),
7 * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
8 * 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:
9 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
10 *
11 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
12 * 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,
13 * 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.
14 */
15 
16 #include <algorithm>
17 #include <cstddef>
18 
19 #include "behaviortree_cpp_v3/controls/parallel_node.h"
20 
21 constexpr const char* BT::ParallelNode::THRESHOLD_FAILURE;
22 constexpr const char* BT::ParallelNode::THRESHOLD_SUCCESS;
23 
24 Parallel_robot::Parallel_robot(const std::string& name, int success_threshold, int failure_threshold) :
25  BT::ControlNode(name, {}),
26  success_threshold_(success_threshold),
27  failure_threshold_(failure_threshold),
28  read_parameter_from_ports_(false){
29  setRegistrationID("Parallel_robot");
30 }
31 
32 Parallel_robot::Parallel_robot(const std::string& name, const BT::NodeConfiguration& config) :
33  BT::ControlNode(name, config),
34  success_threshold_(1),
35  failure_threshold_(1),
36  read_parameter_from_ports_(true)
37 {}
38 
39 BT::NodeStatus Parallel_robot::tick(){
41  if (!getInput(THRESHOLD_SUCCESS, success_threshold_)){
42  throw BT::RuntimeError("Missing parameter [", THRESHOLD_SUCCESS, "] in ParallelNode");
43  }
44 
45  if (!getInput(THRESHOLD_FAILURE, failure_threshold_)){
46  throw BT::RuntimeError("Missing parameter [", THRESHOLD_FAILURE, "] in ParallelNode");
47  }
48  }
49 
50  size_t success_childred_num = 0;
51  size_t failure_childred_num = 0;
52 
53  const size_t children_count = children_nodes_.size();
54 
55  if (children_count < successThreshold()){
56  throw BT::LogicError("Number of children is less than threshold. Can never succeed.");
57  }
58 
59  if (children_count < failureThreshold()){
60  throw BT::LogicError("Number of children is less than threshold. Can never fail.");
61  }
62 
63  // Routing the tree according to the sequence node's logic:
64  for (unsigned int i = 0; i < children_count; i++){
65  TreeNode* child_node = children_nodes_[i];
66 
67  bool in_skip_list = (skip_list_.count(i) != 0);
68 
69  BT::NodeStatus child_status;
70  if (in_skip_list){
71  child_status = child_node->status();
72  }
73  else{
74  child_status = child_node->executeTick();
75  }
76 
77  switch (child_status){
78  case BT::NodeStatus::SUCCESS: {
79  if (!in_skip_list){
80  skip_list_.insert(i);
81  }
82  success_childred_num++;
83 
84  if (success_childred_num == successThreshold()){
85  skip_list_.clear();
86  BT::ControlNode::ControlNode::
87  resetChildren();
88  return BT::NodeStatus::SUCCESS;
89  }
90  }
91  break;
92 
93  case BT::NodeStatus::FAILURE: {
94 
95  // It fails if it is not possible to succeed anymore or if
96  // number of failures are equal to failure_threshold_
97  if ((failure_childred_num > children_count - successThreshold()) ||
98  (failure_childred_num == failureThreshold())){
99  skip_list_.clear();
100  resetChildren();
101  return BT::NodeStatus::FAILURE;
102  }
103  }
104  break;
105 
106  case BT::NodeStatus::RUNNING: {
107  return BT::NodeStatus::RUNNING;
108  }
109  break;
110 
111  default: {
112  throw BT::LogicError("A child node must never return IDLE");
113  }
114  }
115  }
116 
117  return BT::NodeStatus::RUNNING;
118 }
119 
121 {
122  skip_list_.clear();
123  ControlNode::halt();
124 }
125 
127 {
128  return success_threshold_ < 0 ?
129  std::max(children_nodes_.size() + success_threshold_ + 1, size_t(0)) :
131 }
132 
134  return failure_threshold_ < 0 ?
135  std::max(children_nodes_.size() + failure_threshold_ + 1, size_t(0)) :
137 }
138 
140  success_threshold_ = threshold_M;
141 }
142 
144  failure_threshold_ = threshold_M;
145 }
Parallel_robot::tick
virtual BT::NodeStatus tick() override
Definition: parallel_robot.cpp:39
parallel_robot.h
Parallel_robot::THRESHOLD_FAILURE
static constexpr const char * THRESHOLD_FAILURE
Definition: parallel_robot.h:39
Parallel_robot::setFailureThreshold
void setFailureThreshold(int threshold_M)
Definition: parallel_robot.cpp:143
Parallel_robot::setSuccessThreshold
void setSuccessThreshold(int threshold_M)
Definition: parallel_robot.cpp:139
Parallel_robot::skip_list_
std::set< int > skip_list_
Definition: parallel_robot.h:35
Parallel_robot::THRESHOLD_SUCCESS
static constexpr const char * THRESHOLD_SUCCESS
Definition: parallel_robot.h:38
Parallel_robot::failureThreshold
size_t failureThreshold() const
Definition: parallel_robot.cpp:133
Parallel_robot::failure_threshold_
int failure_threshold_
Definition: parallel_robot.h:33
Parallel_robot::success_threshold_
int success_threshold_
Definition: parallel_robot.h:32
Parallel_robot::read_parameter_from_ports_
bool read_parameter_from_ports_
Definition: parallel_robot.h:37
Parallel_robot::Parallel_robot
Parallel_robot(const std::string &name, int success_threshold, int failure_threshold=1)
Definition: parallel_robot.cpp:24
Parallel_robot::successThreshold
size_t successThreshold() const
Definition: parallel_robot.cpp:126
Parallel_robot::halt
virtual void halt() override
Definition: parallel_robot.cpp:120


multi_cell_builder
Author(s): Matteo Anedda
autogenerated on Sun Apr 9 2023 23:59:51