Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #ifndef BEHAVIORTREECORE_TREENODE_H
00015 #define BEHAVIORTREECORE_TREENODE_H
00016
00017 #include <condition_variable>
00018 #include <mutex>
00019 #include "behaviortree_cpp/utils/signal.h"
00020 #include "behaviortree_cpp/exceptions.h"
00021 #include "behaviortree_cpp/basic_types.h"
00022 #include "behaviortree_cpp/blackboard.h"
00023 #include "behaviortree_cpp/utils/strcat.hpp"
00024
00025 #ifdef _MSC_VER
00026 #pragma warning(disable : 4127)
00027 #endif
00028
00029 namespace BT
00030 {
00032 struct TreeNodeManifest
00033 {
00034 NodeType type;
00035 std::string registration_ID;
00036 PortsList ports;
00037 };
00038
00039 typedef std::unordered_map<std::string, std::string> PortsRemapping;
00040
00041 struct NodeConfiguration
00042 {
00043 NodeConfiguration()
00044 {
00045 }
00046
00047 Blackboard::Ptr blackboard;
00048 PortsRemapping input_ports;
00049 PortsRemapping output_ports;
00050 };
00051
00053 class TreeNode
00054 {
00055 public:
00056 typedef std::shared_ptr<TreeNode> Ptr;
00057
00068 TreeNode(std::string name, NodeConfiguration config);
00069
00070 virtual ~TreeNode() = default;
00071
00073 virtual BT::NodeStatus executeTick();
00074
00077 virtual void halt() = 0;
00078
00079 bool isHalted() const;
00080
00081 NodeStatus status() const;
00082
00083 void setStatus(NodeStatus new_status);
00084
00086 const std::string& name() const;
00087
00090 BT::NodeStatus waitValidStatus();
00091
00092 virtual NodeType type() const = 0;
00093
00094 using StatusChangeSignal = Signal<TimePoint, const TreeNode&, NodeStatus, NodeStatus>;
00095 using StatusChangeSubscriber = StatusChangeSignal::Subscriber;
00096 using StatusChangeCallback = StatusChangeSignal::CallableFunction;
00097
00107 StatusChangeSubscriber subscribeToStatusChange(StatusChangeCallback callback);
00108
00109
00110 uint16_t UID() const;
00111
00113 const std::string& registrationName() const;
00114
00117 const NodeConfiguration& config() const;
00118
00126 template <typename T>
00127 Result getInput(const std::string& key, T& destination) const;
00128
00132 template <typename T>
00133 Optional<T> getInput(const std::string& key) const
00134 {
00135 T out;
00136 auto res = getInput(key, out);
00137 return (res) ? Optional<T>(out) : nonstd::make_unexpected(res.error());
00138 }
00139
00140 template <typename T>
00141 Result setOutput(const std::string& key, const T& value);
00142
00145 static bool isBlackboardPointer(StringView str);
00146
00147 static StringView stripBlackboardPointer(StringView str);
00148
00149 static Optional<StringView> getRemappedKey(StringView port_name, StringView remapping_value);
00150
00151 protected:
00153 virtual BT::NodeStatus tick() = 0;
00154
00155 friend class BehaviorTreeFactory;
00156
00157
00158 void setRegistrationID(StringView ID)
00159 {
00160 registration_ID_.assign(ID.data(), ID.size());
00161 }
00162
00163 void modifyPortsRemapping(const PortsRemapping& new_remapping);
00164
00165 private:
00166 const std::string name_;
00167
00168 NodeStatus status_;
00169
00170 std::condition_variable state_condition_variable_;
00171
00172 mutable std::mutex state_mutex_;
00173
00174 StatusChangeSignal state_change_signal_;
00175
00176 const uint16_t uid_;
00177
00178 NodeConfiguration config_;
00179
00180 std::string registration_ID_;
00181 };
00182
00183
00184 template <typename T>
00185 inline Result TreeNode::getInput(const std::string& key, T& destination) const
00186 {
00187 auto remap_it = config_.input_ports.find(key);
00188 if (remap_it == config_.input_ports.end())
00189 {
00190 return nonstd::make_unexpected(StrCat("getInput() failed because "
00191 "NodeConfiguration::input_ports "
00192 "does not contain the key: [",
00193 key, "]"));
00194 }
00195 auto remapped_res = getRemappedKey(key, remap_it->second);
00196 try
00197 {
00198 if (!remapped_res)
00199 {
00200 destination = convertFromString<T>(remap_it->second);
00201 return {};
00202 }
00203 const auto& remapped_key = remapped_res.value();
00204
00205 if (!config_.blackboard)
00206 {
00207 return nonstd::make_unexpected("getInput() trying to access a Blackboard(BB) entry, "
00208 "but BB is invalid");
00209 }
00210
00211 const Any* val = config_.blackboard->getAny(remapped_key.to_string());
00212 if (val && val->empty() == false)
00213 {
00214 if (std::is_same<T, std::string>::value == false && val->type() == typeid(std::string))
00215 {
00216 destination = convertFromString<T>(val->cast<std::string>());
00217 }
00218 else
00219 {
00220 destination = val->cast<T>();
00221 }
00222 return {};
00223 }
00224
00225 return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to find the "
00226 "key [",
00227 key, "] remapped to [", remapped_key, "]"));
00228 }
00229 catch (std::exception& err)
00230 {
00231 return nonstd::make_unexpected(err.what());
00232 }
00233 }
00234
00235 template <typename T>
00236 inline Result TreeNode::setOutput(const std::string& key, const T& value)
00237 {
00238 if (!config_.blackboard)
00239 {
00240 return nonstd::make_unexpected("setOutput() failed: trying to access a "
00241 "Blackboard(BB) entry, but BB is invalid");
00242 }
00243
00244 auto remap_it = config_.output_ports.find(key);
00245 if (remap_it == config_.output_ports.end())
00246 {
00247 return nonstd::make_unexpected(StrCat("setOutput() failed: NodeConfiguration::output_ports "
00248 "does not "
00249 "contain the key: [",
00250 key, "]"));
00251 }
00252 StringView remapped_key = remap_it->second;
00253 if (remapped_key == "=")
00254 {
00255 remapped_key = key;
00256 }
00257 if (isBlackboardPointer(remapped_key))
00258 {
00259 remapped_key = stripBlackboardPointer(remapped_key);
00260 }
00261 const auto& key_str = remapped_key.to_string();
00262
00263 config_.blackboard->set(key_str, value);
00264
00265 return {};
00266 }
00267
00268
00269 template <typename T>
00270 inline void assignDefaultRemapping(NodeConfiguration& config)
00271 {
00272 for (const auto& it : getProvidedPorts<T>())
00273 {
00274 const auto& port_name = it.first;
00275 const auto direction = it.second.direction();
00276 if (direction != PortDirection::OUTPUT)
00277 {
00278 config.input_ports[port_name] = "=";
00279 }
00280 if (direction != PortDirection::INPUT)
00281 {
00282 config.output_ports[port_name] = "=";
00283 }
00284 }
00285 }
00286
00287 }
00288
00289 #endif