00001 /* Copyright (C) 2015-2017 Michele Colledanchise - All Rights Reserved 00002 * 00003 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 00004 * to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 00005 * 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: 00006 * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 00007 * 00008 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 00009 * 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, 00010 * 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. 00011 */ 00012 00013 #include <fallback_node_with_memory.h> 00014 #include <string> 00015 00016 BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name) : ControlNode::ControlNode(name) 00017 { 00018 reset_policy_ = BT::ON_SUCCESS_OR_FAILURE; 00019 current_child_idx_ = 0; // initialize the current running child 00020 } 00021 00022 00023 BT::FallbackNodeWithMemory::FallbackNodeWithMemory(std::string name, int reset_policy) : ControlNode::ControlNode(name) 00024 { 00025 reset_policy_ = reset_policy; 00026 current_child_idx_ = 0; // initialize the current running child 00027 } 00028 00029 00030 BT::FallbackNodeWithMemory::~FallbackNodeWithMemory() {} 00031 00032 00033 BT::ReturnStatus BT::FallbackNodeWithMemory::Tick() 00034 { 00035 DEBUG_STDOUT(get_name() << " ticked, memory counter: "<< current_child_idx_); 00036 00037 // Vector size initialization. N_of_children_ could change at runtime if you edit the tree 00038 N_of_children_ = children_nodes_.size(); 00039 00040 // Routing the ticks according to the fallback node's (with memory) logic: 00041 while (current_child_idx_ < N_of_children_) 00042 { 00043 /* Ticking an action is different from ticking a condition. An action executed some portion of code in another thread. 00044 We want this thread detached so we can cancel its execution (when the action no longer receive ticks). 00045 Hence we cannot just call the method Tick() from the action as doing so will block the execution of the tree. 00046 For this reason if a child of this node is an action, then we send the tick using the tick engine. Otherwise we call the method Tick() and wait for the response. 00047 */ 00048 00049 if (children_nodes_[current_child_idx_]->get_type() == BT::ACTION_NODE) 00050 { 00051 // 1) If the child i is an action, read its state. 00052 // Action nodes runs in another thread, hence you cannot retrieve the status just by executing it. 00053 00054 child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 00055 DEBUG_STDOUT(get_name() << " It is an action " << children_nodes_[current_child_idx_]->get_name() 00056 << " with status: " << child_i_status_); 00057 00058 if (child_i_status_ == BT::IDLE || child_i_status_ == BT::HALTED) 00059 { 00060 // 1.1) If the action status is not running, the sequence node sends a tick to it. 00061 DEBUG_STDOUT(get_name() << "NEEDS TO TICK " << children_nodes_[current_child_idx_]->get_name()); 00062 children_nodes_[current_child_idx_]->tick_engine.Tick(); 00063 00064 // waits for the tick to arrive to the child 00065 do 00066 { 00067 child_i_status_ = children_nodes_[current_child_idx_]->get_status(); 00068 std::this_thread::sleep_for(std::chrono::milliseconds(10)); 00069 } 00070 while (child_i_status_ != BT::RUNNING && child_i_status_ != BT::SUCCESS 00071 && child_i_status_ != BT::FAILURE); 00072 } 00073 } 00074 else 00075 { 00076 // 2) if it's not an action: 00077 // Send the tick and wait for the response; 00078 child_i_status_ = children_nodes_[current_child_idx_]->Tick(); 00079 } 00080 00081 00082 if (child_i_status_ == BT::SUCCESS ||child_i_status_ == BT::FAILURE ) 00083 { 00084 // the child goes in idle if it has returned success or failure. 00085 children_nodes_[current_child_idx_]->set_status(BT::IDLE); 00086 } 00087 00088 if (child_i_status_ != BT::FAILURE) 00089 { 00090 // If the child status is not success, return the status 00091 DEBUG_STDOUT("the status of: " << get_name() << " becomes " << child_i_status_); 00092 if (child_i_status_ == BT::SUCCESS && (reset_policy_ == BT::ON_SUCCESS 00093 || reset_policy_ == BT::ON_SUCCESS_OR_FAILURE)) 00094 { 00095 current_child_idx_ = 0; 00096 } 00097 set_status(child_i_status_); 00098 return child_i_status_; 00099 } 00100 else if (current_child_idx_ != N_of_children_ - 1) 00101 { 00102 // If the child status is failure, continue to the next child 00103 // (if any, hence if(current_child_ != N_of_children_ - 1) ) in the for loop (if any). 00104 current_child_idx_++; 00105 } 00106 else 00107 { 00108 // If it the last child. 00109 if (child_i_status_ == BT::FAILURE) 00110 { 00111 // if it the last child and it has returned failure, reset the memory 00112 current_child_idx_ = 0; 00113 } 00114 set_status(child_i_status_); 00115 return child_i_status_; 00116 } 00117 } 00118 return BT::EXIT; 00119 } 00120 00121 00122 int BT::FallbackNodeWithMemory::DrawType() 00123 { 00124 return BT::SELECTORSTAR; 00125 } 00126 00127 void BT::FallbackNodeWithMemory::Halt() 00128 { 00129 current_child_idx_ = 0; 00130 BT::ControlNode::Halt(); 00131 } 00132 00133