diff --git a/CMakeLists.txt b/CMakeLists.txt index 067d62bc6..68506e0e8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -103,7 +103,9 @@ list(APPEND BT_SOURCE src/decorators/repeat_node.cpp src/decorators/retry_node.cpp src/decorators/timeout_node.cpp + src/decorators/skip_unless_updated.cpp src/decorators/subtree_node.cpp + src/decorators/wait_update.cpp src/controls/if_then_else_node.cpp src/controls/fallback_node.cpp @@ -148,7 +150,8 @@ endif() if (BTCPP_SHARED_LIBS) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE}) + add_library(${BTCPP_LIBRARY} SHARED ${BT_SOURCE} + src/decorators/wait_update.cpp) else() set(CMAKE_POSITION_INDEPENDENT_CODE ON) add_library(${BTCPP_LIBRARY} STATIC ${BT_SOURCE}) diff --git a/include/behaviortree_cpp/behavior_tree.h b/include/behaviortree_cpp/behavior_tree.h index e26cf53e2..346303304 100644 --- a/include/behaviortree_cpp/behavior_tree.h +++ b/include/behaviortree_cpp/behavior_tree.h @@ -33,6 +33,8 @@ #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/decorators/skip_unless_updated.h" +#include "behaviortree_cpp/decorators/wait_update.h" #include "behaviortree_cpp/actions/always_success_node.h" #include "behaviortree_cpp/actions/always_failure_node.h" diff --git a/include/behaviortree_cpp/bt_factory.h b/include/behaviortree_cpp/bt_factory.h index 05ecf9b9d..99b27d4ba 100644 --- a/include/behaviortree_cpp/bt_factory.h +++ b/include/behaviortree_cpp/bt_factory.h @@ -14,14 +14,11 @@ #ifndef BT_FACTORY_H #define BT_FACTORY_H -#include -#include #include #include #include #include #include -#include #include #include "behaviortree_cpp/contrib/magic_enum.hpp" diff --git a/include/behaviortree_cpp/decorators/skip_unless_updated.h b/include/behaviortree_cpp/decorators/skip_unless_updated.h new file mode 100644 index 000000000..62e86aa9f --- /dev/null +++ b/include/behaviortree_cpp/decorators/skip_unless_updated.h @@ -0,0 +1,50 @@ +/* Copyright (C) 2024 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, +* 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. +*/ + +#pragma once + +#include "behaviortree_cpp/decorator_node.h" + +namespace BT +{ + +/** + * @brief The SkipUnlessUpdated checks the Timestamp in an entry + * to determine if the value was updated since the last time (true, + * the first time). + * + * If it is, the child will be executed, otherwise SKIPPED is returned. + */ +class SkipUnlessUpdated : public DecoratorNode +{ +public: + SkipUnlessUpdated(const std::string& name, const NodeConfig& config); + + ~SkipUnlessUpdated() override = default; + + static PortsList providedPorts() + { + return { InputPort("entry", "Skip this branch unless the blackboard value " + "was updated") }; + } + +private: + int64_t sequence_id_ = -1; + std::string entry_key_; + bool still_executing_child_ = false; + + NodeStatus tick() override; + + void halt() override; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/decorators/wait_update.h b/include/behaviortree_cpp/decorators/wait_update.h new file mode 100644 index 000000000..d4d4487d9 --- /dev/null +++ b/include/behaviortree_cpp/decorators/wait_update.h @@ -0,0 +1,49 @@ +/* Copyright (C) 2024 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, +* 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. +*/ + +#pragma once + +#include "behaviortree_cpp/decorator_node.h" + +namespace BT +{ +/** + * @brief The WaitValueUpdate checks the Timestamp in an entry + * to determine if the value was updated since the last time (true, + * the first time). + * + * If it is, the child will be executed, otherwise RUNNING is returned. + */ +class WaitValueUpdate : public DecoratorNode +{ +public: + WaitValueUpdate(const std::string& name, const NodeConfig& config); + + ~WaitValueUpdate() override = default; + + static PortsList providedPorts() + { + return { InputPort("entry", "Sleep until the entry in the blackboard is " + "updated") }; + } + +private: + int64_t sequence_id_ = -1; + std::string entry_key_; + bool still_executing_child_ = false; + + NodeStatus tick() override; + + void halt() override; +}; + +} // namespace BT diff --git a/include/behaviortree_cpp/tree_node.h b/include/behaviortree_cpp/tree_node.h index ef2ba9331..ca38916e0 100644 --- a/include/behaviortree_cpp/tree_node.h +++ b/include/behaviortree_cpp/tree_node.h @@ -467,7 +467,7 @@ inline ResultStamped TreeNode::getInputStamped(const std::string& key, if constexpr(std::is_same_v) { destination = any_value; - return {}; + return Timestamp{ entry->sequence_id, entry->stamp }; } if(!entry->value.empty()) diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index beb6c8b5d..cbd9a59eb 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -95,6 +95,9 @@ BehaviorTreeFactory::BehaviorTreeFactory() : _p(new PImpl) registerNodeType>("LoopDouble"); registerNodeType>("LoopString"); + registerNodeType("SkipUnlessUpdated"); + registerNodeType("WaitValueUpdate"); + for(const auto& it : _p->builders) { _p->builtin_IDs.insert(it.first); diff --git a/src/decorators/skip_unless_updated.cpp b/src/decorators/skip_unless_updated.cpp new file mode 100644 index 000000000..619052bc5 --- /dev/null +++ b/src/decorators/skip_unless_updated.cpp @@ -0,0 +1,64 @@ +/* Copyright (C) 2024 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, +* 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. +*/ + +#pragma once + +#include "behaviortree_cpp/decorators/skip_unless_updated.h" + +namespace BT +{ + +SkipUnlessUpdated::SkipUnlessUpdated(const std::string& name, const NodeConfig& config) + : DecoratorNode(name, config) +{ + const auto entry_str = config.input_ports.at("entry"); + StringView stripped_key; + if(isBlackboardPointer(entry_str, &stripped_key)) + { + entry_key_ = stripped_key; + } + else + { + entry_key_ = entry_str; + } +} + +NodeStatus SkipUnlessUpdated::tick() +{ + // continue executing an asynchronous child + if(still_executing_child_) + { + auto status = child()->executeTick(); + still_executing_child_ = (status == NodeStatus::RUNNING); + return status; + } + + auto entry = config().blackboard->getEntry(entry_key_); + std::unique_lock lk(entry->entry_mutex); + auto seq = static_cast(entry->sequence_id); + if(seq == sequence_id_) + { + return NodeStatus::SKIPPED; + } + sequence_id_ = seq; + + auto status = child()->executeTick(); + still_executing_child_ = (status == NodeStatus::RUNNING); + return status; +} + +void SkipUnlessUpdated::halt() +{ + still_executing_child_ = false; +} + +} // namespace BT diff --git a/src/decorators/wait_update.cpp b/src/decorators/wait_update.cpp new file mode 100644 index 000000000..daf672cae --- /dev/null +++ b/src/decorators/wait_update.cpp @@ -0,0 +1,64 @@ +/* Copyright (C) 2024 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, +* 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. +*/ + +#pragma once + +#include "behaviortree_cpp/decorators/wait_update.h" + +namespace BT +{ + +WaitValueUpdate::WaitValueUpdate(const std::string& name, const NodeConfig& config) + : DecoratorNode(name, config) +{ + const auto entry_str = config.input_ports.at("entry"); + StringView stripped_key; + if(isBlackboardPointer(entry_str, &stripped_key)) + { + entry_key_ = stripped_key; + } + else + { + entry_key_ = entry_str; + } +} + +NodeStatus WaitValueUpdate::tick() +{ + // continue executing an asynchronous child + if(still_executing_child_) + { + auto status = child()->executeTick(); + still_executing_child_ = (status == NodeStatus::RUNNING); + return status; + } + + auto entry = config().blackboard->getEntry(entry_key_); + std::unique_lock lk(entry->entry_mutex); + auto seq = static_cast(entry->sequence_id); + if(seq == sequence_id_) + { + return NodeStatus::RUNNING; + } + sequence_id_ = seq; + + auto status = child()->executeTick(); + still_executing_child_ = (status == NodeStatus::RUNNING); + return status; +} + +void WaitValueUpdate::halt() +{ + still_executing_child_ = false; +} + +} // namespace BT