diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index e80a88a8c..ca73e37c9 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -1,5 +1,5 @@ /* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved - * Copyright (C) 2018-2020 Davide Faconti, Eurecat - All Rights Reserved + * Copyright (C) 2018-2023 Davide Faconti - 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, @@ -11,38 +11,44 @@ * 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. */ -#ifndef BEHAVIOR_TREE_H -#define BEHAVIOR_TREE_H - -#include "behaviortree_cpp_v3/controls/parallel_node.h" -#include "behaviortree_cpp_v3/controls/reactive_sequence.h" -#include "behaviortree_cpp_v3/controls/reactive_fallback.h" -#include "behaviortree_cpp_v3/controls/fallback_node.h" -#include "behaviortree_cpp_v3/controls/sequence_node.h" -#include "behaviortree_cpp_v3/controls/sequence_star_node.h" -#include "behaviortree_cpp_v3/controls/switch_node.h" -#include "behaviortree_cpp_v3/controls/manual_node.h" -#include "behaviortree_cpp_v3/controls/if_then_else_node.h" -#include "behaviortree_cpp_v3/controls/while_do_else_node.h" - -#include "behaviortree_cpp_v3/action_node.h" -#include "behaviortree_cpp_v3/condition_node.h" - -#include "behaviortree_cpp_v3/decorators/inverter_node.h" -#include "behaviortree_cpp_v3/decorators/retry_node.h" -#include "behaviortree_cpp_v3/decorators/repeat_node.h" -#include "behaviortree_cpp_v3/decorators/subtree_node.h" - -#include "behaviortree_cpp_v3/actions/always_success_node.h" -#include "behaviortree_cpp_v3/actions/always_failure_node.h" -#include "behaviortree_cpp_v3/actions/set_blackboard_node.h" - -#include "behaviortree_cpp_v3/decorators/force_success_node.h" -#include "behaviortree_cpp_v3/decorators/force_failure_node.h" -#include "behaviortree_cpp_v3/decorators/keep_running_until_failure_node.h" -#include "behaviortree_cpp_v3/decorators/blackboard_precondition.h" -#include "behaviortree_cpp_v3/decorators/timeout_node.h" -#include "behaviortree_cpp_v3/decorators/delay_node.h" +#pragma once + +#include "behaviortree_cpp/controls/parallel_node.h" +#include "behaviortree_cpp/controls/parallel_all_node.h" +#include "behaviortree_cpp/controls/reactive_sequence.h" +#include "behaviortree_cpp/controls/reactive_fallback.h" +#include "behaviortree_cpp/controls/fallback_node.h" +#include "behaviortree_cpp/controls/sequence_node.h" +#include "behaviortree_cpp/controls/sequence_with_memory_node.h" +#include "behaviortree_cpp/controls/switch_node.h" +#include "behaviortree_cpp/controls/if_then_else_node.h" +#include "behaviortree_cpp/controls/while_do_else_node.h" + +#include "behaviortree_cpp/action_node.h" +#include "behaviortree_cpp/condition_node.h" + +#include "behaviortree_cpp/decorators/inverter_node.h" +#include "behaviortree_cpp/decorators/retry_node.h" +#include "behaviortree_cpp/decorators/repeat_node.h" +#include "behaviortree_cpp/decorators/run_once_node.h" +#include "behaviortree_cpp/decorators/subtree_node.h" +#include "behaviortree_cpp/decorators/loop_node.h" + +#include "behaviortree_cpp/actions/always_success_node.h" +#include "behaviortree_cpp/actions/always_failure_node.h" +#include "behaviortree_cpp/actions/script_condition.h" +#include "behaviortree_cpp/actions/script_node.h" +#include "behaviortree_cpp/actions/set_blackboard_node.h" +#include "behaviortree_cpp/actions/test_node.h" +#include "behaviortree_cpp/actions/sleep_node.h" +#include "behaviortree_cpp/actions/unset_blackboard_node.h" + +#include "behaviortree_cpp/decorators/force_success_node.h" +#include "behaviortree_cpp/decorators/force_failure_node.h" +#include "behaviortree_cpp/decorators/keep_running_until_failure_node.h" +#include "behaviortree_cpp/decorators/script_precondition.h" +#include "behaviortree_cpp/decorators/timeout_node.h" +#include "behaviortree_cpp/decorators/delay_node.h" #include @@ -60,14 +66,15 @@ void applyRecursiveVisitor(const TreeNode* root_node, const std::function& visitor); //Call the visitor for each node of the tree, given a root. -void applyRecursiveVisitor(TreeNode* root_node, const std::function& visitor); +void applyRecursiveVisitor(TreeNode* root_node, + const std::function& visitor); /** * Debug function to print the hierarchy of the tree. Prints to std::cout by default. */ void printTreeRecursively(const TreeNode* root_node, std::ostream& stream = std::cout); -typedef std::vector> SerializedTreeStatus; +using SerializedTreeStatus = std::vector>; /** * @brief buildSerializedStatusSnapshot can be used to create a buffer that can be stored @@ -85,16 +92,18 @@ void buildSerializedStatusSnapshot(const TreeNode* root_node, template inline NodeType getType() { - // clang-format off + // clang-format off if( std::is_base_of::value ) return NodeType::ACTION; if( std::is_base_of::value ) return NodeType::CONDITION; - if( std::is_base_of::value ) return NodeType::SUBTREE; - if( std::is_base_of::value ) return NodeType::SUBTREE; + if( std::is_base_of::value ) return NodeType::SUBTREE; if( std::is_base_of::value ) return NodeType::DECORATOR; if( std::is_base_of::value ) return NodeType::CONTROL; return NodeType::UNDEFINED; - // clang-format on -} + // clang-format on } -#endif // BEHAVIOR_TREE_H +const char* LibraryVersionString(); + +int LibraryVersionNumber(); + +} // namespace BT \ No newline at end of file diff --git a/include/behaviortree_cpp/bt_factory.h b/include/behaviortree_cpp/bt_factory.h index 0380c4984..21932e0f6 100644 --- a/include/behaviortree_cpp/bt_factory.h +++ b/include/behaviortree_cpp/bt_factory.h @@ -233,8 +233,8 @@ class BehaviorTreeFactory auto manifest = CreateManifest(ID); registerBuilder(manifest, builder); } - - /** + +/** * @brief registerSimpleAction help you register nodes of type SimpleActionNode. * * @param ID registration ID @@ -245,7 +245,6 @@ class BehaviorTreeFactory void registerSimpleAction(const std::string& ID, const std::function& tick_bool_functor, PortsList ports = {}); - /** * @brief registerSimpleAction help you register nodes of type SimpleActionNode. * @@ -567,4 +566,4 @@ void ImportTreeFromJSON(const nlohmann::json& json, BT::Tree& tree); } // namespace BT -#endif // BT_FACTORY_H +#endif // BT_FACTORY_H \ No newline at end of file diff --git a/include/behaviortree_cpp/control_node.h b/include/behaviortree_cpp/control_node.h index 558b788d2..c9d22fc3b 100644 --- a/include/behaviortree_cpp/control_node.h +++ b/include/behaviortree_cpp/control_node.h @@ -11,50 +11,54 @@ * 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. */ -#ifndef CONTROLNODE_H -#define CONTROLNODE_H +#pragma once #include -#include "behaviortree_cpp_v3/tree_node.h" +#include "behaviortree_cpp/tree_node.h" namespace BT { class ControlNode : public TreeNode { - protected: - std::vector children_nodes_; +protected: + std::vector children_nodes_; - public: - ControlNode(const std::string& name, const NodeConfiguration& config); +public: + ControlNode(const std::string& name, const NodeConfig& config); - virtual ~ControlNode() override = default; + virtual ~ControlNode() override = default; - /// The method used to add nodes to the children vector - void addChild(TreeNode* child); + /// The method used to add nodes to the children vector + void addChild(TreeNode* child); - size_t childrenCount() const; + void insertChildAfter(TreeNode* child, TreeNode* sibling); - const std::vector& children() const; + size_t childrenCount() const; - const TreeNode* child(size_t index) const - { - return children().at(index); - } + const std::vector& children() const; - virtual void halt() override; + const TreeNode* child(size_t index) const + { + return children().at(index); + } - void haltChildren(); + virtual void halt() override; - [[deprecated( "deprecated: please use explicitly haltChildren() or haltChild(i)")]] - void haltChildren(size_t first); + /// same as resetChildren() + void haltChildren(); - void haltChild(size_t i); + [[deprecated("deprecated: please use explicitly haltChildren() or haltChild(i)")]] void + haltChildren(size_t first); - virtual NodeType type() const override final - { - return NodeType::CONTROL; - } -}; -} + void haltChild(size_t i); + + virtual NodeType type() const override final + { + return NodeType::CONTROL; + } -#endif + /// Set the status of all children to IDLE. + /// also send a halt() signal to all RUNNING children + void resetChildren(); +}; +} // namespace BT \ No newline at end of file diff --git a/include/behaviortree_cpp/controls/sequence_node.h b/include/behaviortree_cpp/controls/sequence_node.h index 2d52dfdfc..29da9b192 100644 --- a/include/behaviortree_cpp/controls/sequence_node.h +++ b/include/behaviortree_cpp/controls/sequence_node.h @@ -50,4 +50,4 @@ class SequenceNode : public ControlNode virtual BT::NodeStatus tick() override; }; -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/include/behaviortree_cpp/decorators/retry_node.h b/include/behaviortree_cpp/decorators/retry_node.h index c5480fd61..0b39626b1 100644 --- a/include/behaviortree_cpp/decorators/retry_node.h +++ b/include/behaviortree_cpp/decorators/retry_node.h @@ -53,19 +53,19 @@ class RetryNode : public DecoratorNode virtual void halt() override; -private: - int max_attempts_; - int try_count_; - bool all_skipped_ = true; - bool is_retrying() const { - return try_count_ > 0; + return try_count_ > 0; } int n_th_retry() const { - return try_count_; + return try_count_; } +private: + int max_attempts_; + int try_count_; + bool all_skipped_ = true; + bool read_parameter_from_ports_; static constexpr const char* NUM_ATTEMPTS = "num_attempts"; @@ -85,4 +85,4 @@ class [[deprecated("RetryUntilSuccesful was a typo and deprecated, use " virtual ~RetryNodeTypo() override = default; }; -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/include/behaviortree_cpp/loggers/bt_cout_logger.h b/include/behaviortree_cpp/loggers/bt_cout_logger.h index bea9a18ab..6308782c3 100644 --- a/include/behaviortree_cpp/loggers/bt_cout_logger.h +++ b/include/behaviortree_cpp/loggers/bt_cout_logger.h @@ -28,4 +28,4 @@ class StdCoutLogger : public StatusChangeLogger } // namespace BT -#endif // BT_COUT_LOGGER_H +#endif // BT_COUT_LOGGER_H \ No newline at end of file diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index cbe436d4a..9bd70e794 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -292,22 +292,6 @@ class TreeNode [[nodiscard]] static Expected getRemappedKey(StringView port_name, StringView remapped_port); - void setParent(TreeNode *parent) { - parent_ = parent; - } - - TreeNode* getParent() const { - return parent_; - } - - std::string short_description() const { - std::string str = name(); - if (str.empty()) { - str = config().uid; - } - return str; - } - /// Notify that the tree should be ticked again() void emitWakeUpSignal(); @@ -336,6 +320,22 @@ class TreeNode } } + void setParent(TreeNode *parent) { + parent_ = parent; + } + + TreeNode* getParent() const { + return parent_; + } + + std::string short_description() const { + std::string str = name(); + if (str.empty()) { + str = config().uid; + } + return str; + } + protected: friend class BehaviorTreeFactory; friend class DecoratorNode; @@ -377,12 +377,12 @@ class TreeNode Expected checkPreConditions(); void checkPostConditions(NodeStatus status); - TreeNode *parent_; - bool failed_ = false; - /// The method used to interrupt the execution of a RUNNING node. /// Only Async nodes that may return RUNNING should implement it. virtual void halt() = 0; + + TreeNode *parent_; + bool failed_ = false; }; //------------------------------------------------------- @@ -575,4 +575,4 @@ inline void assignDefaultRemapping(NodeConfig& config) } } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/include/behaviortree_cpp_v3/bt_factory.h b/include/behaviortree_cpp_v3/bt_factory.h deleted file mode 100644 index f9a030e82..000000000 --- a/include/behaviortree_cpp_v3/bt_factory.h +++ /dev/null @@ -1,410 +0,0 @@ -/* Copyright (C) 2018 Michele Colledanchise - All Rights Reserved - * Copyright (C) 2018-2020 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. -*/ - -#ifndef BT_FACTORY_H -#define BT_FACTORY_H - -#include -#include -#include -#include -#include -#include -#include - - -#include "behaviortree_cpp_v3/behavior_tree.h" - -namespace BT -{ - -/// The term "Builder" refers to the Builder Pattern (https://en.wikipedia.org/wiki/Builder_pattern) -typedef std::function(const std::string&, const NodeConfiguration&)> -NodeBuilder; - -template -using has_default_constructor = typename std::is_constructible; - -template -using has_params_constructor = typename std::is_constructible; - - -template inline - NodeBuilder CreateBuilder(typename std::enable_if::value && - has_params_constructor::value >::type* = nullptr) -{ - return [](const std::string& name, const NodeConfiguration& config) - { - // Special case. Use default constructor if parameters are empty - if( config.input_ports.empty() && - config.output_ports.empty() && - has_default_constructor::value) - { - return std::make_unique(name); - } - return std::make_unique(name, config); - }; -} - -template inline - NodeBuilder CreateBuilder(typename std::enable_if::value && - has_params_constructor::value >::type* = nullptr) -{ - return [](const std::string& name, const NodeConfiguration& params) - { - return std::unique_ptr(new T(name, params)); - }; -} - -template inline - NodeBuilder CreateBuilder(typename std::enable_if::value && - !has_params_constructor::value >::type* = nullptr) -{ - return [](const std::string& name, const NodeConfiguration&) - { - return std::unique_ptr(new T(name)); - }; -} - - -template inline -TreeNodeManifest CreateManifest(const std::string& ID, PortsList portlist = getProvidedPorts()) -{ - return { getType(), ID, portlist }; -} - - -constexpr const char* PLUGIN_SYMBOL = "BT_RegisterNodesFromPlugin"; - -#ifndef BT_PLUGIN_EXPORT - -/* Use this macro to automatically register one or more custom Nodes -into a factory. For instance: - -BT_REGISTER_NODES(factory) -{ - factory.registerNodeType("MoveBase"); -} - -IMPORTANT: this must funtion MUST be declared in a cpp file, NOT a header file. -See examples for more information about configuring CMake correctly -*/ -#define BT_REGISTER_NODES(factory) \ - static void BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory) - -#else - -#if defined(__linux__) || defined __APPLE__ - -#define BT_REGISTER_NODES(factory) \ - extern "C" void __attribute__((visibility("default"))) \ - BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory) - -#elif _WIN32 - -#define BT_REGISTER_NODES(factory) \ - extern "C" void __declspec(dllexport) BT_RegisterNodesFromPlugin(BT::BehaviorTreeFactory& factory) -#endif - -#endif - - -/** - * @brief Struct used to store a tree. - * If this object goes out of scope, the tree is destroyed. - * - * To tick the tree, simply call: - * - * NodeStatus status = my_tree.tickRoot(); - */ -class Tree -{ -public: - - std::vector nodes; - std::vector blackboard_stack; - std::unordered_map manifests; - - Tree(){} - - // non-copyable. Only movable - Tree(const Tree& ) = delete; - Tree& operator=(const Tree&) = delete; - - Tree(Tree&& other) - { - (*this) = std::move(other); - } - - Tree& operator=(Tree&& other) - { - nodes = std::move(other.nodes); - blackboard_stack = std::move(other.blackboard_stack); - manifests = std::move(other.manifests); - return *this; - } - - void haltTree() - { - if(!rootNode()) - { - return; - } - // the halt should propagate to all the node if the nodes - // have been implemented correctly - rootNode()->halt(); - rootNode()->setStatus(NodeStatus::IDLE); - - //but, just in case.... this should be no-op - auto visitor = [](BT::TreeNode * node) { - node->halt(); - node->setStatus(BT::NodeStatus::IDLE); - }; - BT::applyRecursiveVisitor(rootNode(), visitor); - } - - TreeNode* rootNode() const - { - return nodes.empty() ? nullptr : nodes.front().get(); - } - - NodeStatus tickRoot() - { - if(!rootNode()) - { - throw RuntimeError("Empty Tree"); - } - NodeStatus ret = rootNode()->executeTick(); - if( ret == NodeStatus::SUCCESS || ret == NodeStatus::FAILURE){ - rootNode()->setStatus(BT::NodeStatus::IDLE); - } - return ret; - } - - ~Tree(); - - Blackboard::Ptr rootBlackboard(); - -}; - -/** - * @brief The BehaviorTreeFactory is used to create instances of a - * TreeNode at run-time. - * - * Some node types are "builtin", whilst other are used defined and need - * to be registered using a unique ID. - */ -class BehaviorTreeFactory -{ -public: - BehaviorTreeFactory(); - - /// Remove a registered ID. - bool unregisterBuilder(const std::string& ID); - - /// The most generic way to register your own builder. - void registerBuilder(const TreeNodeManifest& manifest, const NodeBuilder& builder); - - template - void registerBuilder(const std::string& ID, const NodeBuilder& builder ) - { - auto manifest = CreateManifest(ID); - registerBuilder(manifest, builder); - } - - /** - * @brief registerSimpleAction help you register nodes of type SimpleActionNode. - * - * @param ID registration ID - * @param tick_bool_functor the callback to be wrapped with NodeStatus return type (true: SUCCESS, false: FAILURE) and invoked in the tick() method. - * @param ports if your SimpleNode requires ports, provide the list here. - * - * */ - void registerSimpleAction(const std::string& ID, - const std::function& tick_bool_functor, - PortsList ports = {}); - /** - * @brief registerSimpleAction help you register nodes of type SimpleActionNode. - * - * @param ID registration ID - * @param tick_functor the callback to be invoked in the tick() method. - * @param ports if your SimpleNode requires ports, provide the list here. - * - * */ - void registerSimpleAction(const std::string& ID, - const SimpleActionNode::TickFunctor& tick_functor, - PortsList ports = {}); - /** - * @brief registerSimpleCondition help you register nodes of type SimpleConditionNode. - * - * @param ID registration ID - * @param tick_bool_functor the callback to be wrapped with NodeStatus return type (true: SUCCESS, false: FAILURE) and invoked in the tick() method. - * @param ports if your SimpleNode requires ports, provide the list here. - * - * */ - void registerSimpleCondition(const std::string &ID, - const std::function &tick_bool_functor, - PortsList ports = {}); - /** - * @brief registerSimpleCondition help you register nodes of type SimpleConditionNode. - * - * @param ID registration ID - * @param tick_functor the callback to be invoked in the tick() method. - * @param ports if your SimpleNode requires ports, provide the list here. - * - * */ - void registerSimpleCondition(const std::string& ID, - const SimpleConditionNode::TickFunctor& tick_functor, - PortsList ports = {}); - /** - * @brief registerSimpleDecorator help you register nodes of type SimpleDecoratorNode. - * - * @param ID registration ID - * @param tick_functor the callback to be invoked in the tick() method. - * @param ports if your SimpleNode requires ports, provide the list here. - * - * */ - void registerSimpleDecorator(const std::string& ID, - const SimpleDecoratorNode::TickFunctor& tick_functor, - PortsList ports = {}); - - /** - * @brief registerFromPlugin load a shared library and execute the function BT_REGISTER_NODES (see macro). - * - * @param file_path path of the file - */ - void registerFromPlugin(const std::string &file_path); - - /** - * @brief registerFromROSPlugins finds all shared libraries that export ROS plugins for behaviortree_cpp, and calls registerFromPlugin for each library. - * @throws If not compiled with ROS support or if the library cannot load for any reason - * - */ - void registerFromROSPlugins(); - - /** - * @brief instantiateTreeNode creates an instance of a previously registered TreeNode. - * - * @param name name of this particular instance - * @param ID ID used when it was registered - * @param config configuration that is passed to the constructor of the TreeNode. - * @return new node. - */ - std::unique_ptr instantiateTreeNode(const std::string& name, const std::string &ID, - const NodeConfiguration& config) const; - - /** registerNodeType is the method to use to register your custom TreeNode. - * - * It accepts only classed derived from either ActionNodeBase, DecoratorNode, - * ControlNode or ConditionNode. - */ - template - void registerNodeType(const std::string& ID) - { - static_assert(std::is_base_of::value || - std::is_base_of::value || - std::is_base_of::value || - std::is_base_of::value, - "[registerNode]: accepts only classed derived from either ActionNodeBase, " - "DecoratorNode, ControlNode or ConditionNode"); - - static_assert(!std::is_abstract::value, - "[registerNode]: Some methods are pure virtual. " - "Did you override the methods tick() and halt()?"); - - constexpr bool default_constructable = std::is_constructible::value; - constexpr bool param_constructable = - std::is_constructible::value; - constexpr bool has_static_ports_list = - has_static_method_providedPorts::value; - - static_assert(default_constructable || param_constructable, - "[registerNode]: the registered class must have at least one of these two " - "constructors: " - " (const std::string&, const NodeConfiguration&) or (const std::string&)."); - - static_assert(!(param_constructable && !has_static_ports_list), - "[registerNode]: you MUST implement the static method: " - " PortsList providedPorts();\n"); - - static_assert(!(has_static_ports_list && !param_constructable), - "[registerNode]: since you have a static method providedPorts(), " - "you MUST add a constructor sign signature (const std::string&, const " - "NodeParameters&)\n"); - - registerBuilder( CreateManifest(ID), CreateBuilder()); - } - - template - void registerNodeType(const std::string& ID, PortsList ports) - { - static_assert(std::is_base_of::value || - std::is_base_of::value || - std::is_base_of::value || - std::is_base_of::value, - "[registerNode]: accepts only classed derived from either ActionNodeBase, " - "DecoratorNode, ControlNode or ConditionNode"); - - static_assert(!std::is_abstract::value, - "[registerNode]: Some methods are pure virtual. " - "Did you override the methods tick() and halt()?"); - - constexpr bool default_constructable = std::is_constructible::value; - constexpr bool param_constructable = - std::is_constructible::value; - constexpr bool has_static_ports_list = - has_static_method_providedPorts::value; - - static_assert(default_constructable || param_constructable, - "[registerNode]: the registered class must have at least one of these two " - "constructors: (const std::string&, const NodeConfiguration&) or (const std::string&)."); - - static_assert(!has_static_ports_list, - "[registerNode]: ports are passed to this node explicitly. The static method" - "providedPorts() should be removed to avoid ambiguities\n"); - - static_assert(param_constructable, - "[registerNode]: since this node has ports, " - "you MUST add a constructor sign signature (const std::string&, const " - "NodeParameters&)\n"); - - registerBuilder( CreateManifest(ID, ports), CreateBuilder()); - } - - /// All the builders. Made available mostly for debug purposes. - const std::unordered_map& builders() const; - - /// Manifests of all the registered TreeNodes. - const std::unordered_map& manifests() const; - - /// List of builtin IDs. - const std::set& builtinNodes() const; - - Tree createTreeFromText(const std::string& text, - Blackboard::Ptr blackboard = Blackboard::create()); - - Tree createTreeFromFile(const std::string& file_path, - Blackboard::Ptr blackboard = Blackboard::create()); - -private: - std::unordered_map builders_; - std::unordered_map manifests_; - std::set builtin_IDs_; - - // clang-format on -}; - - -} // end namespace - -#endif // BT_FACTORY_H diff --git a/include/behaviortree_cpp_v3/tree_node.h b/include/behaviortree_cpp_v3/tree_node.h deleted file mode 100644 index 50a33f534..000000000 --- a/include/behaviortree_cpp_v3/tree_node.h +++ /dev/null @@ -1,316 +0,0 @@ -/* Copyright (C) 2015-2018 Michele Colledanchise - All Rights Reserved -* Copyright (C) 2018-2020 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. -*/ - -#ifndef BEHAVIORTREECORE_TREENODE_H -#define BEHAVIORTREECORE_TREENODE_H - -#include -#include -#include "behaviortree_cpp_v3/utils/signal.h" -#include "behaviortree_cpp_v3/exceptions.h" -#include "behaviortree_cpp_v3/basic_types.h" -#include "behaviortree_cpp_v3/blackboard.h" -#include "behaviortree_cpp_v3/utils/strcat.hpp" - -#ifdef _MSC_VER -#pragma warning(disable : 4127) -#endif - -namespace BT -{ -/// This information is used mostly by the XMLParser. -struct TreeNodeManifest -{ - NodeType type; - std::string registration_ID; - PortsList ports; -}; - -typedef std::unordered_map PortsRemapping; - -struct NodeConfiguration -{ - NodeConfiguration() - { - } - - Blackboard::Ptr blackboard; - PortsRemapping input_ports; - PortsRemapping output_ports; -}; - -/// Abstract base class for Behavior Tree Nodes -class TreeNode -{ - public: - typedef std::shared_ptr Ptr; - - /** - * @brief TreeNode main constructor. - * - * @param name name of the instance, not the type. - * @param config information about input/output ports. See NodeConfiguration - * - * Note: If your custom node has ports, the derived class must implement: - * - * static PortsList providedPorts(); - */ - TreeNode(std::string name, NodeConfiguration config); - - virtual ~TreeNode() = default; - - /// The method that should be used to invoke tick() and setStatus(); - virtual BT::NodeStatus executeTick(); - - /// The method used to interrupt the execution of a RUNNING node. - /// Only Async nodes that may return RUNNING should implement it. - virtual void halt() = 0; - - bool isHalted() const; - - NodeStatus status() const; - - bool has_failed() const; - - /// Name of the instance, not the type - const std::string& name() const; - - /// Blocking function that will sleep until the setStatus() is called with - /// either RUNNING, FAILURE or SUCCESS. - BT::NodeStatus waitValidStatus(); - - virtual NodeType type() const = 0; - - using StatusChangeSignal = Signal; - using StatusChangeSubscriber = StatusChangeSignal::Subscriber; - using StatusChangeCallback = StatusChangeSignal::CallableFunction; - - /** - * @brief subscribeToStatusChange is used to attach a callback to a status change. - * When StatusChangeSubscriber goes out of scope (it is a shared_ptr) the callback - * is unsubscribed automatically. - * - * @param callback The callback to be execute when status change. - * - * @return the subscriber handle. - */ - StatusChangeSubscriber subscribeToStatusChange(StatusChangeCallback callback); - - // get an unique identifier of this instance of treeNode - uint16_t UID() const; - - /// registrationName is the ID used by BehaviorTreeFactory to create an instance. - const std::string& registrationName() const; - - /// Configuration passed at construction time. Can never change after the - /// creation of the TreeNode instance. - const NodeConfiguration& config() const; - - /** Read an input port, which, in practice, is an entry in the blackboard. - * If the blackboard contains a std::string and T is not a string, - * convertFromString() is used automatically to parse the text. - * - * @param key the identifier (before remapping) of the port. - * @return false if an error occurs. - */ - template - Result getInput(const std::string& key, T& destination) const; - - /** Same as bool getInput(const std::string& key, T& destination) - * but using optional. - */ - template - Optional getInput(const std::string& key) const - { - T out; - auto res = getInput(key, out); - return (res) ? Optional(out) : nonstd::make_unexpected(res.error()); - } - - template - Result setOutput(const std::string& key, const T& value); - - // function provide mostrly for debugging purpose to see the raw value - // in the port (no remapping and no conversion to a type) - StringView getRawPortValue(const std::string &key) const; - - /// Check a string and return true if it matches either one of these - /// two patterns: {...} or ${...} - static bool isBlackboardPointer(StringView str); - - static StringView stripBlackboardPointer(StringView str); - - static Optional getRemappedKey(StringView port_name, StringView remapping_value); - - void setParent(TreeNode *parent) { - parent_ = parent; - } - - TreeNode* getParent() const { - return parent_; - } - - std::string short_description() const { - std::string str = name(); - if (str.empty()) { - str = registration_ID_; - } - return str; - } - - protected: - /// Method to be implemented by the user - virtual BT::NodeStatus tick() = 0; - - friend class BehaviorTreeFactory; - friend class DecoratorNode; - friend class ControlNode; - friend class Tree; - - // Only BehaviorTreeFactory should call this - void setRegistrationID(StringView ID) - { - registration_ID_.assign(ID.data(), ID.size()); - } - - void modifyPortsRemapping(const PortsRemapping& new_remapping); - - void setStatus(NodeStatus new_status); - - private: - const std::string name_; - - TreeNode *parent_; - - NodeStatus status_; - - bool failed_ = false; - - std::condition_variable state_condition_variable_; - - mutable std::mutex state_mutex_; - - StatusChangeSignal state_change_signal_; - - const uint16_t uid_; - - NodeConfiguration config_; - - std::string registration_ID_; -}; - -//------------------------------------------------------- -template -inline Result TreeNode::getInput(const std::string& key, T& destination) const -{ - auto remap_it = config_.input_ports.find(key); - if (remap_it == config_.input_ports.end()) - { - return nonstd::make_unexpected(StrCat("getInput() failed because " - "NodeConfiguration::input_ports " - "does not contain the key: [", - key, "]")); - } - auto remapped_res = getRemappedKey(key, remap_it->second); - try - { - if (!remapped_res) - { - destination = convertFromString(remap_it->second); - return {}; - } - const auto& remapped_key = remapped_res.value(); - - if (!config_.blackboard) - { - return nonstd::make_unexpected("getInput() trying to access a Blackboard(BB) entry, " - "but BB is invalid"); - } - - const Any* val = config_.blackboard->getAny(static_cast(remapped_key)); - if (val && val->empty() == false) - { - if (std::is_same::value == false && val->type() == typeid(std::string)) - { - destination = convertFromString(val->cast()); - } - else - { - destination = val->cast(); - } - return {}; - } - - return nonstd::make_unexpected(StrCat("getInput() failed because it was unable to find the " - "key [", - key, "] remapped to [", remapped_key, "]")); - } - catch (std::exception& err) - { - return nonstd::make_unexpected(err.what()); - } -} - -template -inline Result TreeNode::setOutput(const std::string& key, const T& value) -{ - if (!config_.blackboard) - { - return nonstd::make_unexpected("setOutput() failed: trying to access a " - "Blackboard(BB) entry, but BB is invalid"); - } - - auto remap_it = config_.output_ports.find(key); - if (remap_it == config_.output_ports.end()) - { - return nonstd::make_unexpected(StrCat("setOutput() failed: NodeConfiguration::output_ports " - "does not " - "contain the key: [", - key, "]")); - } - StringView remapped_key = remap_it->second; - if (remapped_key == "=") - { - remapped_key = key; - } - if (isBlackboardPointer(remapped_key)) - { - remapped_key = stripBlackboardPointer(remapped_key); - } - config_.blackboard->set(static_cast(remapped_key), value); - - return {}; -} - -// Utility function to fill the list of ports using T::providedPorts(); -template -inline void assignDefaultRemapping(NodeConfiguration& config) -{ - for (const auto& it : getProvidedPorts()) - { - const auto& port_name = it.first; - const auto direction = it.second.direction(); - if (direction != PortDirection::OUTPUT) - { - config.input_ports[port_name] = "="; - } - if (direction != PortDirection::INPUT) - { - config.output_ports[port_name] = "="; - } - } -} - -} // namespace BT - -#endif diff --git a/src/behavior_tree.cpp b/src/behavior_tree.cpp index 92a7b0781..b448c144d 100644 --- a/src/behavior_tree.cpp +++ b/src/behavior_tree.cpp @@ -15,7 +15,7 @@ namespace BT { - + void applyRecursiveVisitorSelectively(const TreeNode* node, const std::function& visitor) { if (!node) @@ -64,7 +64,6 @@ void applyRecursiveVisitorSelectively(TreeNode* node, const std::function& visitor) { @@ -117,18 +116,18 @@ void printTreeRecursively(const TreeNode* root_node, std::ostream& stream) { std::function recursivePrint; - recursivePrint = [&recursivePrint, &stream](unsigned indent, const BT::TreeNode* node) { - for (unsigned i = 0; i < indent; i++) - { - stream << " "; - } - if (!node) - { - stream << "!nullptr!" << std::endl; - return; - } - stream << node->short_description() << std::endl; - indent++; + recursivePrint = [&recursivePrint, &stream](unsigned indent, const BT::TreeNode* node) { + for(unsigned i = 0; i < indent; i++) + { + stream << " "; + } + if(!node) + { + stream << "!nullptr!" << std::endl; + return; + } + stream << node->short_description() << std::endl; + indent++; if(auto control = dynamic_cast(node)) { @@ -178,4 +177,4 @@ const char* LibraryVersionString() return BTCPP_LIBRARY_VERSION; } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 8c8a95bd0..98a377423 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -157,9 +157,9 @@ void BehaviorTreeFactory::registerSimpleCondition(const std::string& ID, registerSimpleCondition(ID, tick_functor, ports); } -void BehaviorTreeFactory::registerSimpleCondition(const std::string& ID, - const SimpleConditionNode::TickFunctor& tick_functor, - PortsList ports) +void BehaviorTreeFactory::registerSimpleCondition( + const std::string& ID, const SimpleConditionNode::TickFunctor& tick_functor, + PortsList ports) { NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeConfig& config) { @@ -181,9 +181,9 @@ void BehaviorTreeFactory::registerSimpleAction(const std::string& ID, registerSimpleAction(ID, tick_functor, ports); } -void BehaviorTreeFactory::registerSimpleAction(const std::string& ID, - const SimpleActionNode::TickFunctor& tick_functor, - PortsList ports) +void BehaviorTreeFactory::registerSimpleAction( + const std::string& ID, const SimpleActionNode::TickFunctor& tick_functor, + PortsList ports) { NodeBuilder builder = [tick_functor, ID](const std::string& name, const NodeConfig& config) { @@ -470,7 +470,21 @@ void BehaviorTreeFactory::addMetadataToManifest(const std::string& node_id, void BehaviorTreeFactory::registerScriptingEnum(StringView name, int value) { - (*_p->scripting_enums)[std::string(name)] = value; + const auto str = std::string(name); + auto it = _p->scripting_enums->find(str); + if(it == _p->scripting_enums->end()) + { + _p->scripting_enums->insert({ str, value }); + } + else + { + if(it->second != value) + { + throw LogicError( + StrCat("Registering the enum [", name, "] twice with different values, first ", + std::to_string(it->second), " and later ", std::to_string(value))); + } + } } void BehaviorTreeFactory::clearSubstitutionRules() @@ -682,20 +696,6 @@ NodeStatus Tree::tickRoot(TickOption opt, std::chrono::milliseconds sleep_time) return status; } -// void BlackboardClone(const Blackboard& src, Blackboard& dst) -// { -// dst.clear(); -// for(auto const key_name : src.getKeys()) -// { -// const auto key = std::string(key_name); -// const auto entry = src.getEntry(key); -// dst.createEntry(key, entry->info); -// auto new_entry = dst.getEntry(key); -// new_entry->value = entry->value; -// new_entry->string_converter = entry->string_converter; -// } -// } - void BlackboardRestore(const std::vector& backup, Tree& tree) { assert(backup.size() == tree.subtrees.size()); @@ -741,4 +741,4 @@ void ImportTreeFromJSON(const nlohmann::json& json, Tree& tree) } } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/condition_node.cpp b/src/condition_node.cpp index 481b64bbb..1d6b585c8 100644 --- a/src/condition_node.cpp +++ b/src/condition_node.cpp @@ -27,19 +27,19 @@ SimpleConditionNode::SimpleConditionNode(const std::string& name, NodeStatus SimpleConditionNode::tick() { - NodeStatus prev_status = status(); + NodeStatus prev_status = status(); - if (prev_status == NodeStatus::IDLE) - { - setStatus(NodeStatus::RUNNING); - prev_status = NodeStatus::RUNNING; - } + if (prev_status == NodeStatus::IDLE) + { + setStatus(NodeStatus::RUNNING); + prev_status = NodeStatus::RUNNING; + } - NodeStatus status = tick_functor_(*this); - if (status != prev_status) - { - setStatus(status); - } - return status; + NodeStatus status = tick_functor_(*this); + if (status != prev_status) + { + setStatus(status); + } + return status; } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/control_node.cpp b/src/control_node.cpp index ffe1e06a9..bdf2214fb 100644 --- a/src/control_node.cpp +++ b/src/control_node.cpp @@ -21,8 +21,8 @@ ControlNode::ControlNode(const std::string& name, const NodeConfig& config) void ControlNode::addChild(TreeNode* child) { - children_nodes_.push_back(child); - child->setParent(this); + children_nodes_.push_back(child); + child->setParent(this); } void ControlNode::insertChildAfter(TreeNode* child, TreeNode* sibling) { @@ -85,4 +85,4 @@ void ControlNode::haltChildren(size_t first) } } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/controls/sequence_node.cpp b/src/controls/sequence_node.cpp index 09513443f..9530fd8c4 100644 --- a/src/controls/sequence_node.cpp +++ b/src/controls/sequence_node.cpp @@ -43,18 +43,10 @@ TreeNode* SequenceNode::getNextSibling(TreeNode* child) { } } -TreeNode* SequenceNode::getNextSibling(TreeNode* child) { - ptrdiff_t i = distance(children_nodes_.begin(), find(children_nodes_.begin(), children_nodes_.end(), child)); - if ( i + 1 < childrenCount()) { - return children_nodes_.at(i + 1); - } - else { - return nullptr; - } -} - NodeStatus SequenceNode::tick() { + const size_t children_count = children_nodes_.size(); + if(status() == NodeStatus::IDLE) { all_skipped_ = true; @@ -63,10 +55,7 @@ NodeStatus SequenceNode::tick() setStatus(NodeStatus::RUNNING); TreeNode* current_child_node = children_nodes_[0]; - do { - TreeNode* current_child_node = children_nodes_[current_child_idx_]; - auto prev_status = current_child_node->status(); const NodeStatus child_status = current_child_node->executeTick(); @@ -113,9 +102,8 @@ NodeStatus SequenceNode::tick() //haltChildren(0); resetChildren(); current_child_idx_ = 0; - // Skip if ALL the nodes have been skipped return all_skipped_ ? NodeStatus::SKIPPED : NodeStatus::SUCCESS; } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/decorator_node.cpp b/src/decorator_node.cpp index 0e8b1cccd..0c346958e 100644 --- a/src/decorator_node.cpp +++ b/src/decorator_node.cpp @@ -26,8 +26,8 @@ void DecoratorNode::setChild(TreeNode* child) throw BehaviorTreeException("Decorator [", name(), "] has already a child assigned"); } - child_node_ = child; - child->setParent(this); + child_node_ = child; + child->setParent(this); } void DecoratorNode::halt() @@ -86,4 +86,4 @@ NodeStatus DecoratorNode::executeTick() return status; } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/loggers/bt_cout_logger.cpp b/src/loggers/bt_cout_logger.cpp index 82d0a1766..40e473aa5 100644 --- a/src/loggers/bt_cout_logger.cpp +++ b/src/loggers/bt_cout_logger.cpp @@ -25,13 +25,11 @@ void StdCoutLogger::callback(Duration timestamp, const TreeNode& node, constexpr const char* whitespaces = " "; constexpr const size_t ws_count = 25; - double since_epoch = duration(timestamp).count(); - printf("[%.3f]: %s%s %s -> %s", - since_epoch, node.short_description().c_str(), - &whitespaces[std::min(ws_count, node.short_description().size())], - toStr(prev_status, true).c_str(), - toStr(status, true).c_str() ); - std::cout << std::endl; + double since_epoch = duration(timestamp).count(); + printf("[%.3f]: %s%s %s -> %s", since_epoch, node.short_description().c_str(), + &whitespaces[std::min(ws_count, node.short_description().size())], + toStr(prev_status, true).c_str(), toStr(status, true).c_str()); + std::cout << std::endl; } void StdCoutLogger::flush() @@ -40,4 +38,4 @@ void StdCoutLogger::flush() ref_count = false; } -} // namespace BT +} // namespace BT \ No newline at end of file diff --git a/src/tree_node.cpp b/src/tree_node.cpp index fd054e53e..ca22d7956 100644 --- a/src/tree_node.cpp +++ b/src/tree_node.cpp @@ -155,8 +155,8 @@ void TreeNode::setStatus(NodeStatus new_status) "]: you are not allowed to set manually the status to IDLE. " "If you know what you are doing (?) use resetStatus() instead."); } - if(new_status == NodeStatus::FAILURE) { - this->failed_ = true; + if (new_status == NodeStatus::FAILURE) { + this->failed_ = true; } NodeStatus prev_status; @@ -499,4 +499,4 @@ AnyPtrLocked BT::TreeNode::getLockedPortContent(const std::string& key) return {}; } -} // namespace BT +} // namespace BT \ No newline at end of file