diff --git a/include/behaviortree_cpp/actions/sleep_node.h b/include/behaviortree_cpp/actions/sleep_node.h index 337bf9a27..06370723c 100644 --- a/include/behaviortree_cpp/actions/sleep_node.h +++ b/include/behaviortree_cpp/actions/sleep_node.h @@ -20,6 +20,9 @@ class SleepNode : public StatefulActionNode ~SleepNode() override { + // Join the timer thread first: timer_ is the first member, so it is destroyed + // after delay_mutex_ and timer_waiting_, which its handler touches. + timer_.stop(); halt(); } @@ -40,13 +43,12 @@ class SleepNode : public StatefulActionNode } private: + // Member order is frozen for ABI compatibility: do not reorder. + TimerQueue<> timer_; uint64_t timer_id_ = 0; std::atomic_bool timer_waiting_ = false; std::mutex delay_mutex_; - // Keep last: ~TimerQueue() joins the worker thread, so this must be destroyed - // before delay_mutex_ and timer_waiting_, which the timer handler touches. - TimerQueue<> timer_; }; } // namespace BT diff --git a/include/behaviortree_cpp/decorators/delay_node.h b/include/behaviortree_cpp/decorators/delay_node.h index e1e610ddc..4e5dd8151 100644 --- a/include/behaviortree_cpp/decorators/delay_node.h +++ b/include/behaviortree_cpp/decorators/delay_node.h @@ -48,7 +48,9 @@ class DelayNode : public DecoratorNode // so the child can outlive or predecease this decorator. halt() -> // DecoratorNode::halt() -> resetChild() dereferences that child and // segfaults on libc++ (macOS). This mirrors TimeoutNode's destructor. - timer_.cancelAll(); + // Join the timer thread here: timer_ is the first member, so it is destroyed + // after delay_mutex_, which its handler locks. + timer_.stop(); } DelayNode(const DelayNode&) = delete; @@ -65,6 +67,8 @@ class DelayNode : public DecoratorNode void halt() override; private: + // Member order is frozen for ABI compatibility: do not reorder. + TimerQueue<> timer_; uint64_t timer_id_; virtual BT::NodeStatus tick() override; @@ -75,9 +79,6 @@ class DelayNode : public DecoratorNode unsigned msec_; bool read_parameter_from_ports_; std::mutex delay_mutex_; - // Keep last: ~TimerQueue() joins the worker thread, so this must be destroyed - // before delay_mutex_, which the timer handler locks. - TimerQueue<> timer_; }; } // namespace BT diff --git a/include/behaviortree_cpp/decorators/timeout_node.h b/include/behaviortree_cpp/decorators/timeout_node.h index 3b0f88d82..935f8eaa8 100644 --- a/include/behaviortree_cpp/decorators/timeout_node.h +++ b/include/behaviortree_cpp/decorators/timeout_node.h @@ -62,7 +62,10 @@ class TimeoutNode : public DecoratorNode const std::unique_lock lk(timeout_mutex_); timeout_started_ = false; } - timer_.cancelAll(); + // Join the timer thread here, without holding timeout_mutex_ (the handler + // locks it): timer_ is the first member, so it is destroyed after + // timeout_mutex_ and timeout_started_. + timer_.stop(); } TimeoutNode(const TimeoutNode&) = delete; @@ -81,6 +84,8 @@ class TimeoutNode : public DecoratorNode void halt() override; + // Member order is frozen for ABI compatibility: do not reorder. + TimerQueue<> timer_; std::atomic_bool child_halted_ = false; uint64_t timer_id_; @@ -88,9 +93,6 @@ class TimeoutNode : public DecoratorNode bool read_parameter_from_ports_; std::atomic_bool timeout_started_ = false; std::mutex timeout_mutex_; - // Keep last: ~TimerQueue() joins the worker thread, so this must be destroyed - // before timeout_mutex_, which the timer handler locks. - TimerQueue<> timer_; }; } // namespace BT diff --git a/include/behaviortree_cpp/utils/timer_queue.h b/include/behaviortree_cpp/utils/timer_queue.h index 04769fdc9..0eadd35f5 100644 --- a/include/behaviortree_cpp/utils/timer_queue.h +++ b/include/behaviortree_cpp/utils/timer_queue.h @@ -84,6 +84,14 @@ class TimerQueue } ~TimerQueue() + { + stop(); + } + + //! Cancels all timers, runs their handlers (aborted=true) and joins the worker + //! thread: after it returns no handler is running or will run. Idempotent. + //! Timers added after stop() are never executed. + void stop() { // Cancel before raising m_finish: once the worker sees the flag, every // item is already due, so its final checkWork() drains the whole queue. diff --git a/src/actions/sleep_node.cpp b/src/actions/sleep_node.cpp index be52f7070..8944c6997 100644 --- a/src/actions/sleep_node.cpp +++ b/src/actions/sleep_node.cpp @@ -1,5 +1,7 @@ #include "behaviortree_cpp/actions/sleep_node.h" +#include + namespace BT { @@ -22,20 +24,29 @@ NodeStatus SleepNode::onStart() setStatus(NodeStatus::RUNNING); + // TimerQueue::cancel() cannot stop a handler that the worker thread already + // picked up: after a halt() and a restart, the handler of the previous timer + // may still run, with aborted == false. Each handler therefore checks that it + // belongs to the current timer. Holding delay_mutex_ across add() and the + // assignment of timer_id_ means the handler, which locks it too, can only + // compare after both its own id and timer_id_ are set. + auto id = std::make_shared(0); + const std::unique_lock lk(delay_mutex_); timer_waiting_ = true; - - timer_id_ = timer_.add(std::chrono::milliseconds(msec), [this](bool aborted) { - // An aborted handler runs later on the timer thread, possibly after - // onStart() armed a new timer: it must not clear timer_waiting_, which - // onHalted() already did. + timer_id_ = timer_.add(std::chrono::milliseconds(msec), [this, id](bool aborted) { if(aborted) { return; } const std::unique_lock lk(delay_mutex_); + if(*id != timer_id_) + { + return; + } timer_waiting_ = false; emitWakeUpSignal(); }); + *id = timer_id_; return NodeStatus::RUNNING; } @@ -47,8 +58,11 @@ NodeStatus SleepNode::onRunning() void SleepNode::onHalted() { + const std::unique_lock lk(delay_mutex_); timer_waiting_ = false; timer_.cancel(timer_id_); + // a handler that the worker thread already picked up must not wake us up + timer_id_ = 0; } } // namespace BT diff --git a/src/decorators/delay_node.cpp b/src/decorators/delay_node.cpp index 601c2ea15..82538d28c 100644 --- a/src/decorators/delay_node.cpp +++ b/src/decorators/delay_node.cpp @@ -3,6 +3,8 @@ #include "behaviortree_cpp/decorators/delay_node.h" +#include + namespace BT { DelayNode::DelayNode(const std::string& name, unsigned milliseconds) @@ -20,8 +22,14 @@ DelayNode::DelayNode(const std::string& name, const NodeConfig& config) void DelayNode::halt() { - delay_started_ = false; - timer_.cancelAll(); + { + const std::unique_lock lk(delay_mutex_); + delay_started_ = false; + timer_.cancelAll(); + // a handler that the worker thread already picked up must not complete + // the delay + timer_id_ = 0; + } DecoratorNode::halt(); } @@ -35,6 +43,8 @@ NodeStatus DelayNode::tick() } } + std::unique_lock lk(delay_mutex_); + if(!delay_started_) { delay_complete_ = false; @@ -42,18 +52,25 @@ NodeStatus DelayNode::tick() delay_started_ = true; setStatus(NodeStatus::RUNNING); - timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) { + // TimerQueue::cancel() cannot stop a handler that the worker thread + // already picked up: after a halt() and a restart, the handler of the + // previous timer may still run, with aborted == false. Each handler + // therefore checks that it belongs to the current timer. Holding + // delay_mutex_ across add() and the assignment of timer_id_ means the + // handler, which locks it too, can only compare after both are set. + auto id = std::make_shared(0); + timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this, id](bool aborted) { const std::unique_lock lk(delay_mutex_); - delay_complete_ = (!aborted); - if(!aborted) + if(aborted || *id != timer_id_) { - emitWakeUpSignal(); + return; } + delay_complete_ = true; + emitWakeUpSignal(); }); + *id = timer_id_; } - const std::unique_lock lk(delay_mutex_); - if(delay_aborted_) { delay_aborted_ = false; diff --git a/src/decorators/timeout_node.cpp b/src/decorators/timeout_node.cpp index f96cc115c..67889bfb9 100644 --- a/src/decorators/timeout_node.cpp +++ b/src/decorators/timeout_node.cpp @@ -12,6 +12,8 @@ #include "behaviortree_cpp/decorators/timeout_node.h" +#include + namespace BT { @@ -25,6 +27,8 @@ NodeStatus TimeoutNode::tick() } } + std::unique_lock lk(timeout_mutex_); + if(!timeout_started_) { timeout_started_ = true; @@ -33,7 +37,15 @@ NodeStatus TimeoutNode::tick() if(msec_ > 0) { - timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this](bool aborted) { + // TimerQueue::cancel() cannot stop a handler that the worker thread + // already picked up: after the child completed (or a halt()) and a + // restart, the handler of the previous timer may still run, with + // aborted == false. Each handler therefore checks that it belongs to + // the current timer. Holding timeout_mutex_ across add() and the + // assignment of timer_id_ means the handler, which locks it too, can + // only compare after both are set. + auto id = std::make_shared(0); + timer_id_ = timer_.add(std::chrono::milliseconds(msec_), [this, id](bool aborted) { // Return immediately if the timer was aborted. // This function could be invoked during destruction of this object and // we don't want to access member variables if not needed. @@ -44,7 +56,7 @@ NodeStatus TimeoutNode::tick() const std::unique_lock lk(timeout_mutex_); // halt() may have run between the timer firing and this lock: the // child was already halted and may not exist anymore. - if(!timeout_started_) + if(!timeout_started_ || *id != timer_id_) { return; } @@ -55,11 +67,10 @@ NodeStatus TimeoutNode::tick() emitWakeUpSignal(); } }); + *id = timer_id_; } } - std::unique_lock lk(timeout_mutex_); - if(child_halted_) { timeout_started_ = false; @@ -69,9 +80,8 @@ NodeStatus TimeoutNode::tick() if(isStatusCompleted(child_status)) { timeout_started_ = false; - lk.unlock(); timer_.cancel(timer_id_); - lk.lock(); + timer_id_ = 0; resetChild(); } return child_status; @@ -86,6 +96,7 @@ void TimeoutNode::halt() const std::unique_lock lk(timeout_mutex_); timeout_started_ = false; timer_.cancelAll(); + timer_id_ = 0; } DecoratorNode::halt(); } diff --git a/tests/gtest_decorator.cpp b/tests/gtest_decorator.cpp index d434c8069..d6706dc7b 100644 --- a/tests/gtest_decorator.cpp +++ b/tests/gtest_decorator.cpp @@ -595,6 +595,90 @@ TEST(Decorator, SleepRestartedAfterHaltWaitsFullDuration) } } +// Regression test: TimerQueue::cancel() cannot stop a handler that the worker +// thread already picked up. Here the timeout expires while the tick thread +// holds the Timeout mutex (ticking the slow child), so its handler is stuck on +// that mutex. The child then succeeds and the Timeout restarts with a new +// timer: the stale handler used to see the new RUNNING child and halt it +// long before the new timeout. +TEST(Decorator, TimeoutRestartedIgnoresStaleTimerHandler) +{ + struct Counters + { + int starts = 0; + std::atomic halts{ 0 }; + }; + + // the 1st, 3rd, ... start succeeds after a while; the others keep running + class SlowThenRunning : public BT::StatefulActionNode + { + public: + SlowThenRunning(const std::string& name, const BT::NodeConfig& config, + Counters* counters) + : StatefulActionNode(name, config), counters_(counters) + {} + + static BT::PortsList providedPorts() + { + return {}; + } + + NodeStatus onStart() override + { + if(counters_->starts++ % 2 == 0) + { + // let the 5 ms timeout expire while the Timeout mutex is held + std::this_thread::sleep_for(milliseconds(30)); + // the restarted Timeout gets a long timeout + config().blackboard->set("msec", 1000u); + return NodeStatus::SUCCESS; + } + return NodeStatus::RUNNING; + } + + NodeStatus onRunning() override + { + return NodeStatus::RUNNING; + } + + void onHalted() override + { + ++counters_->halts; + } + + private: + Counters* counters_; + }; + + Counters counters; + BT::BehaviorTreeFactory factory; + factory.registerNodeType("SlowThenRunning", &counters); + + const std::string xml_text = R"( + + + + + + + )"; + + auto tree = factory.createTreeFromText(xml_text); + + for(int i = 0; i < 10; i++) + { + tree.rootBlackboard()->set("msec", 5u); + ASSERT_EQ(tree.tickOnce(), NodeStatus::SUCCESS); + // restart right away, while the expired handler waits for the mutex + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING); + std::this_thread::sleep_for(milliseconds(20)); + ASSERT_EQ(counters.halts.load(), i) << "iteration " << i; + ASSERT_EQ(tree.tickOnce(), NodeStatus::RUNNING) << "iteration " << i; + tree.haltTree(); + ASSERT_EQ(counters.halts.load(), i + 1); + } +} + // Test KeepRunningUntilFailure decorator TEST(Decorator, KeepRunningUntilFailure) {