diff --git a/CMakeLists.txt b/CMakeLists.txt index 6cb7e5679..6224bc965 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -161,7 +161,6 @@ list(APPEND BT_SOURCE src/decorators/repeat_node.cpp src/decorators/retry_node.cpp src/decorators/subtree_node.cpp - src/decorators/timeout_node.cpp src/decorators/delay_node.cpp src/controls/if_then_else_node.cpp diff --git a/include/behaviortree_cpp_v3/decorators/timeout_node.h b/include/behaviortree_cpp_v3/decorators/timeout_node.h index ca1197433..ac345795c 100644 --- a/include/behaviortree_cpp_v3/decorators/timeout_node.h +++ b/include/behaviortree_cpp_v3/decorators/timeout_node.h @@ -10,7 +10,7 @@ namespace BT /** * @brief The TimeoutNode will halt() a running child if * the latter has been RUNNING for more than a give time. - * The timeout is in millisecons and it is passed using the port "msec". + * The timeout is in milliseconds and it is passed using the port "msec". * * If timeout is reached it returns FAILURE. * @@ -20,12 +20,30 @@ namespace BT * * */ +template class TimeoutNode : public DecoratorNode { public: - TimeoutNode(const std::string& name, unsigned milliseconds); + TimeoutNode(const std::string& name, unsigned milliseconds) + : DecoratorNode(name, {} ), + child_halted_(false), + timer_id_(0), + msec_(milliseconds), + read_parameter_from_ports_(false), + timeout_started_(false) + { + setRegistrationID("Timeout"); + } - TimeoutNode(const std::string& name, const NodeConfiguration& config); + TimeoutNode(const std::string& name, const NodeConfiguration& config) + : DecoratorNode(name, config), + child_halted_(false), + timer_id_(0), + msec_(0), + read_parameter_from_ports_(true), + timeout_started_(false) + { + } ~TimeoutNode() override { @@ -39,9 +57,59 @@ class TimeoutNode : public DecoratorNode } private: - TimerQueue timer_ ; + TimerQueue<_Clock, _Duration> timer_ ; + + virtual BT::NodeStatus tick() override + { + if( read_parameter_from_ports_ ) + { + if( !getInput("msec", msec_) ) + { + throw RuntimeError("Missing parameter [msec] in TimeoutNode"); + } + } + + if ( !timeout_started_ ) + { + timeout_started_ = true; + setStatus(NodeStatus::RUNNING); + child_halted_ = false; + + if (msec_ > 0) + { + timer_id_ = timer_.add(std::chrono::milliseconds(msec_), + [this](bool aborted) + { + std::unique_lock lk( timeout_mutex_ ); + if (!aborted && child()->status() == NodeStatus::RUNNING) + { + child_halted_ = true; + haltChild(); + } + }); + } + } - virtual BT::NodeStatus tick() override; + std::unique_lock lk( timeout_mutex_ ); + + if (child_halted_) + { + timeout_started_ = false; + return NodeStatus::FAILURE; + } + else + { + auto child_status = child()->executeTick(); + if (child_status != NodeStatus::RUNNING) + { + timeout_started_ = false; + timeout_mutex_.unlock(); + timer_.cancel(timer_id_); + timeout_mutex_.lock(); + } + return child_status; + } + } std::atomic child_halted_; uint64_t timer_id_; diff --git a/include/behaviortree_cpp_v3/decorators/timer_queue.h b/include/behaviortree_cpp_v3/decorators/timer_queue.h index abc9ab19d..5c1fc4ed6 100644 --- a/include/behaviortree_cpp_v3/decorators/timer_queue.h +++ b/include/behaviortree_cpp_v3/decorators/timer_queue.h @@ -62,6 +62,7 @@ class Semaphore // - Handlers are ALWAYS executed in the Timer Queue worker thread. // - Handlers execution order is NOT guaranteed // +template class TimerQueue { public: @@ -85,7 +86,7 @@ class TimerQueue uint64_t add(std::chrono::milliseconds milliseconds, std::function handler) { WorkItem item; - item.end = Clock::now() + milliseconds; + item.end = _Clock::now() + milliseconds; item.handler = std::move(handler); std::unique_lock lk(m_mtx); @@ -118,7 +119,7 @@ class TimerQueue { WorkItem newItem; // Zero time, so it stays at the top for immediate execution - newItem.end = Clock::time_point(); + newItem.end = std::chrono::time_point<_Clock, _Duration>(); newItem.id = 0; // Means it is a canceled item // Move the handler from item to newitem. // Also, we need to manually set the handler to nullptr, since @@ -150,7 +151,7 @@ class TimerQueue { if (item.id) { - item.end = Clock::time_point(); + item.end = std::chrono::time_point<_Clock, _Duration>(); item.id = 0; } } @@ -162,7 +163,6 @@ class TimerQueue } private: - using Clock = std::chrono::steady_clock; TimerQueue(const TimerQueue&) = delete; TimerQueue& operator=(const TimerQueue&) = delete; @@ -193,7 +193,7 @@ class TimerQueue assert(m_items.size() == 0); } - std::pair calcWaitTime() + std::pair> calcWaitTime() { std::lock_guard lk(m_mtx); while (m_items.size()) @@ -212,13 +212,13 @@ class TimerQueue // No items found, so return no wait time (causes the thread to wait // indefinitely) - return std::make_pair(false, Clock::time_point()); + return std::make_pair(false, std::chrono::time_point<_Clock, _Duration>()); } void checkWork() { std::unique_lock lk(m_mtx); - while (m_items.size() && m_items.top().end <= Clock::now()) + while (m_items.size() && m_items.top().end <= _Clock::now()) { WorkItem item(std::move(m_items.top())); m_items.pop(); @@ -237,7 +237,7 @@ class TimerQueue struct WorkItem { - Clock::time_point end; + std::chrono::time_point<_Clock, _Duration> end; uint64_t id; // id==0 means it was cancelled std::function handler; bool operator>(const WorkItem& other) const diff --git a/src/bt_factory.cpp b/src/bt_factory.cpp index e0b3f8f40..704bdfc73 100644 --- a/src/bt_factory.cpp +++ b/src/bt_factory.cpp @@ -36,7 +36,7 @@ BehaviorTreeFactory::BehaviorTreeFactory() registerNodeType("RetryUntilSuccesful"); registerNodeType("KeepRunningUntilFailure"); registerNodeType("Repeat"); - registerNodeType("Timeout"); + registerNodeType>("Timeout"); registerNodeType("Delay"); registerNodeType("ForceSuccess"); diff --git a/src/decorators/timeout_node.cpp b/src/decorators/timeout_node.cpp deleted file mode 100644 index 69905a980..000000000 --- a/src/decorators/timeout_node.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/* 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_v3/decorators/timeout_node.h" -#include "behaviortree_cpp_v3/action_node.h" - -namespace BT -{ - -TimeoutNode::TimeoutNode(const std::string& name, unsigned milliseconds) - : DecoratorNode(name, {} ), - child_halted_(false), - timer_id_(0), - msec_(milliseconds), - read_parameter_from_ports_(false), - timeout_started_(false) -{ - setRegistrationID("Timeout"); -} - -TimeoutNode::TimeoutNode(const std::string& name, const NodeConfiguration& config) - : DecoratorNode(name, config), - child_halted_(false), - timer_id_(0), - msec_(0), - read_parameter_from_ports_(true), - timeout_started_(false) -{ -} - -NodeStatus TimeoutNode::tick() -{ - if( read_parameter_from_ports_ ) - { - if( !getInput("msec", msec_) ) - { - throw RuntimeError("Missing parameter [msec] in TimeoutNode"); - } - } - - if ( !timeout_started_ ) - { - timeout_started_ = true; - setStatus(NodeStatus::RUNNING); - child_halted_ = false; - - if (msec_ > 0) - { - timer_id_ = timer_.add(std::chrono::milliseconds(msec_), - [this](bool aborted) - { - std::unique_lock lk( timeout_mutex_ ); - if (!aborted && child()->status() == NodeStatus::RUNNING) - { - child_halted_ = true; - haltChild(); - } - }); - } - } - - std::unique_lock lk( timeout_mutex_ ); - - if (child_halted_) - { - timeout_started_ = false; - return NodeStatus::FAILURE; - } - else - { - auto child_status = child()->executeTick(); - if (child_status != NodeStatus::RUNNING) - { - timeout_started_ = false; - timeout_mutex_.unlock(); - timer_.cancel(timer_id_); - timeout_mutex_.lock(); - } - return child_status; - } -} - -} diff --git a/tests/gtest_coroutines.cpp b/tests/gtest_coroutines.cpp index 77364856f..d006582c8 100644 --- a/tests/gtest_coroutines.cpp +++ b/tests/gtest_coroutines.cpp @@ -116,7 +116,7 @@ TEST(CoroTest, do_action_timeout) BT::assignDefaultRemapping(node_config_); SimpleCoroAction node( milliseconds(300), false, "Action", node_config_); - BT::TimeoutNode timeout("TimeoutAction", 200); + BT::TimeoutNode<> timeout("TimeoutAction", 200); timeout.setChild(&node); @@ -137,7 +137,7 @@ TEST(CoroTest, sequence_child) SimpleCoroAction actionA( milliseconds(200), false, "action_A", node_config_); SimpleCoroAction actionB( milliseconds(200), false, "action_B", node_config_); - BT::TimeoutNode timeout("timeout", 300); + BT::TimeoutNode<> timeout("timeout", 300); BT::SequenceNode sequence("sequence"); timeout.setChild(&sequence); diff --git a/tests/gtest_decorator.cpp b/tests/gtest_decorator.cpp index b8eaf3cbf..e07470b9d 100644 --- a/tests/gtest_decorator.cpp +++ b/tests/gtest_decorator.cpp @@ -20,7 +20,7 @@ using std::chrono::milliseconds; struct DeadlineTest : testing::Test { - BT::TimeoutNode root; + BT::TimeoutNode<> root; BT::AsyncActionTest action; DeadlineTest() : root("deadline", 300) @@ -66,7 +66,7 @@ struct RetryTest : testing::Test struct TimeoutAndRetry : testing::Test { - BT::TimeoutNode timeout_root; + BT::TimeoutNode<> timeout_root; BT::RetryNode retry; BT::SyncActionTest action;