diff --git a/CMakeLists.txt b/CMakeLists.txt index 067d62bc6..fdb3a41d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,7 @@ list(APPEND BT_SOURCE src/bt_factory.cpp src/decorator_node.cpp src/condition_node.cpp + src/assumption_checker_node.cpp src/control_node.cpp src/shared_library.cpp src/tree_node.cpp diff --git a/include/behaviortree_cpp/assumption_checker_node.h b/include/behaviortree_cpp/assumption_checker_node.h new file mode 100644 index 000000000..d9440da46 --- /dev/null +++ b/include/behaviortree_cpp/assumption_checker_node.h @@ -0,0 +1,63 @@ +/* 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 ASSUMPTIONCHECKERNODE_H +#define ASSUMPTIONCHECKERNODE_H + +#include "leaf_node.h" +#include "behaviortree_cpp/condition_node.h" + +namespace BT +{ +class AssumptionCheckerNode : public ConditionNode +{ +public: + AssumptionCheckerNode(const std::string& name, const NodeConfig& config); + + virtual ~AssumptionCheckerNode() override = default; + + virtual NodeType type() const override final + { + return NodeType::ASSUMPTIONCHECKER; + } +}; + +/** + * @brief The SimpleAssumptionCheckerNode provides an easy to use AssumptionCheckerNode. + * The user should simply provide a callback with this signature + * + * BT::NodeStatus functionName(void) + * + * This avoids the hassle of inheriting from a ActionNode. + * + * Using lambdas or std::bind it is easy to pass a pointer to a method. + */ +class SimpleAssumptionCheckerNode : public AssumptionCheckerNode +{ +public: + using TickFunctor = std::function; + + // You must provide the function to call when tick() is invoked + SimpleAssumptionCheckerNode(const std::string& name, TickFunctor tick_functor, + const NodeConfig& config); + + ~SimpleAssumptionCheckerNode() override = default; + +protected: + virtual NodeStatus tick() override; + + TickFunctor tick_functor_; +}; +} // namespace BT + +#endif diff --git a/include/behaviortree_cpp/basic_types.h b/include/behaviortree_cpp/basic_types.h index 18a3a931c..cc6a65529 100644 --- a/include/behaviortree_cpp/basic_types.h +++ b/include/behaviortree_cpp/basic_types.h @@ -22,6 +22,7 @@ enum class NodeType UNDEFINED = 0, ACTION, CONDITION, + ASSUMPTIONCHECKER, CONTROL, DECORATOR, SUBTREE diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index ca73e37c9..b4779fb35 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -26,6 +26,7 @@ #include "behaviortree_cpp/action_node.h" #include "behaviortree_cpp/condition_node.h" +#include "behaviortree_cpp/assumption_checker_node.h" #include "behaviortree_cpp/decorators/inverter_node.h" #include "behaviortree_cpp/decorators/retry_node.h" @@ -95,6 +96,7 @@ inline NodeType getType() // 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::ASSUMPTIONCHECKER; 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; diff --git a/include/behaviortree_cpp/bt_factory.h b/include/behaviortree_cpp/bt_factory.h index 21932e0f6..ea43c6c91 100644 --- a/include/behaviortree_cpp/bt_factory.h +++ b/include/behaviortree_cpp/bt_factory.h @@ -278,6 +278,28 @@ class BehaviorTreeFactory void registerSimpleCondition(const std::string& ID, const SimpleConditionNode::TickFunctor& tick_functor, PortsList ports = {}); + /** + * @brief registerSimpleAssumptionChecker help you register nodes of type SimpleAssumptionCheckerNode. + * + * @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 registerSimpleAssumptionChecker(const std::string &ID, + const std::function &tick_bool_functor, + PortsList ports = {}); + /** + * @brief registerSimpleAssumptionChecker help you register nodes of type SimpleAssumptionCheckerNode. + * + * @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 registerSimpleAssumptionChecker(const std::string& ID, + const SimpleAssumptionCheckerNode::TickFunctor& tick_functor, + PortsList ports = {}); /** * @brief registerSimpleDecorator help you register nodes of type SimpleDecoratorNode. * diff --git a/include/behaviortree_cpp/condition_node.h b/include/behaviortree_cpp/condition_node.h index 3c6299d2a..78b8ca157 100644 --- a/include/behaviortree_cpp/condition_node.h +++ b/include/behaviortree_cpp/condition_node.h @@ -31,7 +31,7 @@ class ConditionNode : public LeafNode resetStatus(); } - virtual NodeType type() const override final + virtual NodeType type() const override { return NodeType::CONDITION; } diff --git a/src/assumption_checker_node.cpp b/src/assumption_checker_node.cpp new file mode 100644 index 000000000..572b45274 --- /dev/null +++ b/src/assumption_checker_node.cpp @@ -0,0 +1,45 @@ +/* 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. +*/ + +#include "behaviortree_cpp/assumption_checker_node.h" + +namespace BT +{ +AssumptionCheckerNode::AssumptionCheckerNode(const std::string& name, const NodeConfig& config) + : ConditionNode::ConditionNode(name, config) +{} + +SimpleAssumptionCheckerNode::SimpleAssumptionCheckerNode(const std::string& name, + TickFunctor tick_functor, + const NodeConfig& config) + : AssumptionCheckerNode(name, config), tick_functor_(std::move(tick_functor)) +{} + +NodeStatus SimpleAssumptionCheckerNode::tick() +{ + NodeStatus prev_status = status(); + + 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; +} +} // namespace BT \ No newline at end of file diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index 98a377423..fa373b141 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -170,6 +170,30 @@ void BehaviorTreeFactory::registerSimpleCondition( registerBuilder(manifest, builder); } +void BehaviorTreeFactory::registerSimpleAssumptionChecker(const std::string& ID, + const std::function& tick_bool_functor, + PortsList ports) +{ + auto tick_functor = [tick_bool_functor](TreeNode& parent_node) -> NodeStatus { + return tick_bool_functor(parent_node) ? NodeStatus::SUCCESS : NodeStatus::FAILURE; + }; + + registerSimpleAssumptionChecker(ID, tick_functor, ports); +} + +void BehaviorTreeFactory::registerSimpleAssumptionChecker( + const std::string& ID, const SimpleAssumptionCheckerNode::TickFunctor& tick_functor, + PortsList ports) +{ + NodeBuilder builder = [tick_functor, ID](const std::string& name, + const NodeConfig& config) { + return std::make_unique(name, tick_functor, config); + }; + + TreeNodeManifest manifest = { NodeType::ASSUMPTIONCHECKER, ID, std::move(ports), {} }; + registerBuilder(manifest, builder); +} + void BehaviorTreeFactory::registerSimpleAction(const std::string& ID, const std::function& tick_bool_functor, PortsList ports)