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;