Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "behaviortree_cpp/tree_node.h"
00015 #include <cstring>
00016
00017 namespace BT
00018 {
00019 static uint16_t getUID()
00020 {
00021 static uint16_t uid = 1;
00022 return uid++;
00023 }
00024
00025 TreeNode::TreeNode(std::string name, NodeConfiguration config)
00026 : name_(std::move(name)),
00027 status_(NodeStatus::IDLE),
00028 uid_(getUID()),
00029 config_(std::move(config))
00030 {
00031 }
00032
00033 NodeStatus TreeNode::executeTick()
00034 {
00035 const NodeStatus status = tick();
00036 setStatus(status);
00037 return status;
00038 }
00039
00040 void TreeNode::setStatus(NodeStatus new_status)
00041 {
00042 NodeStatus prev_status;
00043 {
00044 std::unique_lock<std::mutex> UniqueLock(state_mutex_);
00045 prev_status = status_;
00046 status_ = new_status;
00047 }
00048 if (prev_status != new_status)
00049 {
00050 state_condition_variable_.notify_all();
00051 state_change_signal_.notify(std::chrono::high_resolution_clock::now(), *this, prev_status,
00052 new_status);
00053 }
00054 }
00055
00056 NodeStatus TreeNode::status() const
00057 {
00058 std::lock_guard<std::mutex> lock(state_mutex_);
00059 return status_;
00060 }
00061
00062 NodeStatus TreeNode::waitValidStatus()
00063 {
00064 std::unique_lock<std::mutex> lock(state_mutex_);
00065
00066 while( isHalted() )
00067 {
00068 state_condition_variable_.wait(lock);
00069 }
00070 return status_;
00071 }
00072
00073 const std::string& TreeNode::name() const
00074 {
00075 return name_;
00076 }
00077
00078 bool TreeNode::isHalted() const
00079 {
00080 return status_ == NodeStatus::IDLE;
00081 }
00082
00083 TreeNode::StatusChangeSubscriber
00084 TreeNode::subscribeToStatusChange(TreeNode::StatusChangeCallback callback)
00085 {
00086 return state_change_signal_.subscribe(std::move(callback));
00087 }
00088
00089 uint16_t TreeNode::UID() const
00090 {
00091 return uid_;
00092 }
00093
00094 const std::string& TreeNode::registrationName() const
00095 {
00096 return registration_ID_;
00097 }
00098
00099 const NodeConfiguration &TreeNode::config() const
00100 {
00101 return config_;
00102 }
00103
00104 bool TreeNode::isBlackboardPointer(StringView str)
00105 {
00106 const auto size = str.size();
00107 if( size >= 3 && str.back() == '}')
00108 {
00109 if( str[0] == '{') {
00110 return true;
00111 }
00112 if( size >= 4 && str[0] == '$' && str[1] == '{') {
00113 return true;
00114 }
00115 }
00116 return false;
00117 }
00118
00119 StringView TreeNode::stripBlackboardPointer(StringView str)
00120 {
00121 const auto size = str.size();
00122 if( size >= 3 && str.back() == '}')
00123 {
00124 if( str[0] == '{') {
00125 return str.substr(1, size-2);
00126 }
00127 if( str[0] == '$' && str[1] == '{') {
00128 return str.substr(2, size-3);
00129 }
00130 }
00131 return {};
00132 }
00133
00134 Optional<StringView> TreeNode::getRemappedKey(StringView port_name, StringView remapping_value)
00135 {
00136 if( remapping_value == "=" )
00137 {
00138 return {port_name};
00139 }
00140 if( isBlackboardPointer( remapping_value ) )
00141 {
00142 return {stripBlackboardPointer(remapping_value)};
00143 }
00144 return nonstd::make_unexpected("Not a blackboard pointer");
00145 }
00146
00147 void TreeNode::modifyPortsRemapping(const PortsRemapping &new_remapping)
00148 {
00149 for (const auto& new_it: new_remapping)
00150 {
00151 auto it = config_.input_ports.find( new_it.first );
00152 if( it != config_.input_ports.end() )
00153 {
00154 it->second = new_it.second;
00155 }
00156 it = config_.output_ports.find( new_it.first );
00157 if( it != config_.output_ports.end() )
00158 {
00159 it->second = new_it.second;
00160 }
00161 }
00162 }
00163
00164 }