21648313606

Committed 03 Feb 2026 09:20PM UTC coverage: 77.327% (+7.7%) from 69.604%

Build # 21648313606

Build Type

Pull #1102

github

Committed by

web-flow

Commit Message

Merge cb33b3a23 into 2626fc59f

Pull Request Pull Request #1102: Improve test suite quality and coverage

Run Details

35 of 35 new or added lines in 4 files covered. (100.0%)

1 existing line in 1 file now uncovered.

4195 of 5425 relevant lines covered (77.33%)

23903.83 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

98.96

/include/behaviortree_cpp/utils/timer_queue.h

#pragma once

#include <atomic>
#include <chrono>
#include <condition_variable>
#include <functional>
#include <mutex>
#include <queue>
#include <thread>

#include <assert.h>

namespace BT
{
// http://www.crazygaze.com/blog/2016/03/24/portable-c-timer-queue/

namespace details
{
class Semaphore
{
public:
  Semaphore(unsigned int count = 0) : m_count(count)
  {}

  void notify()
  {
    m_count.fetch_add(1);
    m_cv.notify_one();
  }

  template <class Clock, class Duration>
  bool waitUntil(const std::chrono::time_point<Clock, Duration>& point)
  {
    std::unique_lock<std::mutex> lock(m_mtx);
    if(!m_cv.wait_until(lock, point, [this]() { return m_count > 0 || m_unlock; }))
    {
      return false;
    }
    // Only decrement if there is a real count. If we woke because of manualUnlock,
    // m_count may be zero and we must not decrement it.
    if(m_count > 0)
    {
      m_count.fetch_sub(1);
    }
    // Clear the manual unlock flag
    m_unlock = false;

    return true;
  }

  void manualUnlock()
  {
    m_unlock = true;
    m_cv.notify_one();
  }

private:
  std::mutex m_mtx;
  std::condition_variable m_cv;
  std::atomic_uint m_count = 0;
  std::atomic_bool m_unlock = false;
};
}  // namespace details

// Timer Queue
//
// Allows execution of handlers at a specified time in the future
// Guarantees:
//  - All handlers are executed ONCE, even if canceled (aborted parameter will
//be set to true)
//      - If TimerQueue is destroyed, it will cancel all handlers.
//  - Handlers are ALWAYS executed in the Timer Queue worker thread.
//  - Handlers execution order is NOT guaranteed
//
template <typename ClockT = std::chrono::steady_clock,
          typename DurationT = std::chrono::steady_clock::duration>
class TimerQueue
{
public:
  TimerQueue()
  {
    m_finish.store(false);
    m_thread = std::thread([this]() { run(); });
  }

  ~TimerQueue()
  {
    m_finish.store(true);
    cancelAll();
    if(m_thread.joinable())
    {
      m_thread.join();
    }
  }

  //! Adds a new timer
  // \return
  //  Returns the ID of the new timer. You can use this ID to cancel the
  // timer
  uint64_t add(std::chrono::milliseconds milliseconds, std::function<void(bool)> handler)
  {
    WorkItem item;
    item.end = ClockT::now() + milliseconds;
    item.handler = std::move(handler);

    std::unique_lock<std::mutex> lk(m_mtx);
    uint64_t id = ++m_idcounter;
    item.id = id;
    m_items.push(std::move(item));
    lk.unlock();

    // Something changed, so wake up timer thread
    m_checkWork.notify();
    return id;
  }

  //! Cancels the specified timer
  // \return
  //  1 if the timer was cancelled.
  //  0 if you were too late to cancel (or the timer ID was never valid to
  // start with)
  size_t cancel(uint64_t id)
  {
    // Instead of removing the item from the container (thus breaking the
    // heap integrity), we set the item as having no handler, and put
    // that handler on a new item at the top for immediate execution
    // The timer thread will then ignore the original item, since it has no
    // handler.
    std::unique_lock<std::mutex> lk(m_mtx);
    for(auto&& item : m_items.getContainer())
    {
      if(item.id == id && item.handler)
      {
        WorkItem newItem;
        // Zero time, so it stays at the top for immediate execution
        newItem.end = std::chrono::time_point<ClockT, DurationT>();
        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
        // the standard does not guarantee moving an std::function will
        // empty it. Some STL implementation will empty it, others will
        // not.
        newItem.handler = std::move(item.handler);
        item.handler = nullptr;
        m_items.push(std::move(newItem));

        lk.unlock();
        // Something changed, so wake up timer thread
        m_checkWork.notify();
        return 1;
      }
    }
    return 0;
  }

  //! Cancels all timers
  // \return
  //  The number of timers cancelled
  size_t cancelAll()
  {
    // Setting all "end" to 0 (for immediate execution) is ok,
    // since it maintains the heap integrity
    std::unique_lock<std::mutex> lk(m_mtx);
    for(auto&& item : m_items.getContainer())
    {
      if(item.id)
      {
        item.end = std::chrono::time_point<ClockT, DurationT>();
        item.id = 0;
      }
    }
    auto ret = m_items.size();

    lk.unlock();
    m_checkWork.notify();
    return ret;
  }

  TimerQueue(const TimerQueue&) = delete;
  TimerQueue& operator=(const TimerQueue&) = delete;
  TimerQueue(TimerQueue&&) = delete;
  TimerQueue& operator=(TimerQueue&&) = delete;

private:
  void run()
  {
    while(!m_finish.load())
    {
      auto end = calcWaitTime();
      if(end.first)
      {
        // Timers found, so wait until it expires (or something else
        // changes)
        m_checkWork.waitUntil(end.second);
      }
      else
      {
        // No timers exist, so wait an arbitrary amount of time
        m_checkWork.waitUntil(ClockT::now() + std::chrono::milliseconds(10));
      }

      // Check and execute as much work as possible, such as, all expired
      // timers
      checkWork();
    }

    // If we are shutting down, we should not have any items left,
    // since the shutdown cancels all items
    assert(m_items.size() == 0);
  }

  std::pair<bool, std::chrono::time_point<ClockT, DurationT>> calcWaitTime()
  {
    std::lock_guard<std::mutex> lk(m_mtx);
    while(m_items.size())
    {
      if(m_items.top().handler)
      {
        // Item present, so return the new wait time
        return std::make_pair(true, m_items.top().end);
      }
      else
      {
        // Discard empty handlers (they were cancelled)
        m_items.pop();
      }
    }

    // No items found, so return no wait time (causes the thread to wait
    // indefinitely)
    return std::make_pair(false, std::chrono::time_point<ClockT, DurationT>());
  }

  void checkWork()
  {
    std::unique_lock<std::mutex> lk(m_mtx);
    while(m_items.size() && m_items.top().end <= ClockT::now())
    {
      WorkItem item(std::move(m_items.top()));
      m_items.pop();

      lk.unlock();
      if(item.handler)
      {
        item.handler(item.id == 0);
      }
      lk.lock();
    }
  }

  details::Semaphore m_checkWork;
  std::thread m_thread;
  std::atomic_bool m_finish = false;
  uint64_t m_idcounter = 0;

  struct WorkItem
  {
    std::chrono::time_point<ClockT, DurationT> end;
    uint64_t id = 0;  // id==0 means it was cancelled
    std::function<void(bool)> handler;
    bool operator>(const WorkItem& other) const
    {
      return end > other.end;
    }
  };

  std::mutex m_mtx;
  // Inheriting from priority_queue, so we can access the internal container
  class Queue
    : public std::priority_queue<WorkItem, std::vector<WorkItem>, std::greater<WorkItem>>
  {
  public:
    std::vector<WorkItem>& getContainer()
    {
      return this->c;
    }
  } m_items;
};
}  // namespace BT

1	#pragma once
2
3	#include <atomic>
4	#include <chrono>
5	#include <condition_variable>
6	#include <functional>
7	#include <mutex>
8	#include <queue>
9	#include <thread>
10
11	#include <assert.h>
12
13	namespace BT
14	{
15	// http://www.crazygaze.com/blog/2016/03/24/portable-c-timer-queue/
16
17	namespace details
18	{
19	class Semaphore
20	{
21	public:
22	Semaphore(unsigned int count = 0) : m_count(count)	30✔
23	{}	30✔
24
25	void notify()	1,261✔
26	{
27	m_count.fetch_add(1);	1,261✔
28	m_cv.notify_one();	1,261✔
29	}	1,261✔
30
31	template <class Clock, class Duration>
32	bool waitUntil(const std::chrono::time_point<Clock, Duration>& point)	1,362✔
33	{
34	std::unique_lock<std::mutex> lock(m_mtx);	1,362✔
35	if(!m_cv.wait_until(lock, point, [this]() { return m_count > 0 \|\| m_unlock; }))	4,085✔
36	{
37	return false;	109✔
38	}
39	// Only decrement if there is a real count. If we woke because of manualUnlock,
40	// m_count may be zero and we must not decrement it.
41	if(m_count > 0)	1,253✔
42	{
43	m_count.fetch_sub(1);	1,253✔
44	}
45	// Clear the manual unlock flag
46	m_unlock = false;	1,253✔
47
48	return true;	1,253✔
49	}	1,362✔
50
51	void manualUnlock()
52	{
53	m_unlock = true;
54	m_cv.notify_one();
55	}
56
57	private:
58	std::mutex m_mtx;
59	std::condition_variable m_cv;
60	std::atomic_uint m_count = 0;
61	std::atomic_bool m_unlock = false;
62	};
63	} // namespace details
64
65	// Timer Queue
66	//
67	// Allows execution of handlers at a specified time in the future
68	// Guarantees:
69	// - All handlers are executed ONCE, even if canceled (aborted parameter will
70	//be set to true)
71	// - If TimerQueue is destroyed, it will cancel all handlers.
72	// - Handlers are ALWAYS executed in the Timer Queue worker thread.
73	// - Handlers execution order is NOT guaranteed
74	//
75	template <typename ClockT = std::chrono::steady_clock,
76	typename DurationT = std::chrono::steady_clock::duration>
77	class TimerQueue
78	{
79	public:
80	TimerQueue()	30✔
81	{	30✔
82	m_finish.store(false);	30✔
83	m_thread = std::thread([this]() { run(); });	60✔
84	}	30✔
85
86	~TimerQueue()	30✔
87	{
88	m_finish.store(true);	30✔
89	cancelAll();	30✔
90	if(m_thread.joinable())	30✔
91	{
92	m_thread.join();	30✔
93	}
94	}	30✔
95
96	//! Adds a new timer
97	// \return
98	// Returns the ID of the new timer. You can use this ID to cancel the
99	// timer
100	uint64_t add(std::chrono::milliseconds milliseconds, std::function<void(bool)> handler)	623✔
101	{
102	WorkItem item;	623✔
103	item.end = ClockT::now() + milliseconds;	623✔
104	item.handler = std::move(handler);	623✔
105
106	std::unique_lock<std::mutex> lk(m_mtx);	623✔
107	uint64_t id = ++m_idcounter;	623✔
108	item.id = id;	623✔
109	m_items.push(std::move(item));	623✔
110	lk.unlock();	623✔
111
112	// Something changed, so wake up timer thread
113	m_checkWork.notify();	623✔
114	return id;	623✔
115	}	623✔
116
117	//! Cancels the specified timer
118	// \return
119	// 1 if the timer was cancelled.
120	// 0 if you were too late to cancel (or the timer ID was never valid to
121	// start with)
122	size_t cancel(uint64_t id)	595✔
123	{
124	// Instead of removing the item from the container (thus breaking the
125	// heap integrity), we set the item as having no handler, and put
126	// that handler on a new item at the top for immediate execution
127	// The timer thread will then ignore the original item, since it has no
128	// handler.
129	std::unique_lock<std::mutex> lk(m_mtx);	595✔
130	for(auto&& item : m_items.getContainer())	595✔
131	{
132	if(item.id == id && item.handler)	595✔
133	{
134	WorkItem newItem;	595✔
135	// Zero time, so it stays at the top for immediate execution
136	newItem.end = std::chrono::time_point<ClockT, DurationT>();	595✔
137	newItem.id = 0; // Means it is a canceled item	595✔
138	// Move the handler from item to newItem.
139	// Also, we need to manually set the handler to nullptr, since
140	// the standard does not guarantee moving an std::function will
141	// empty it. Some STL implementation will empty it, others will
142	// not.
143	newItem.handler = std::move(item.handler);	595✔
144	item.handler = nullptr;	595✔
145	m_items.push(std::move(newItem));	595✔
146
147	lk.unlock();	595✔
148	// Something changed, so wake up timer thread
149	m_checkWork.notify();	595✔
150	return 1;	595✔
151	}	595✔
152	}
UNCOV 153	return 0;	×
154	}	595✔
155
156	//! Cancels all timers
157	// \return
158	// The number of timers cancelled
159	size_t cancelAll()	43✔
160	{
161	// Setting all "end" to 0 (for immediate execution) is ok,
162	// since it maintains the heap integrity
163	std::unique_lock<std::mutex> lk(m_mtx);	43✔
164	for(auto&& item : m_items.getContainer())	44✔
165	{
166	if(item.id)	1✔
167	{
168	item.end = std::chrono::time_point<ClockT, DurationT>();	1✔
169	item.id = 0;	1✔
170	}
171	}
172	auto ret = m_items.size();	43✔
173
174	lk.unlock();	43✔
175	m_checkWork.notify();	43✔
176	return ret;	43✔
177	}	43✔
178
179	TimerQueue(const TimerQueue&) = delete;
180	TimerQueue& operator=(const TimerQueue&) = delete;
181	TimerQueue(TimerQueue&&) = delete;
182	TimerQueue& operator=(TimerQueue&&) = delete;
183
184	private:
185	void run()	30✔
186	{
187	while(!m_finish.load())	1,392✔
188	{
189	auto end = calcWaitTime();	1,362✔
190	if(end.first)	1,362✔
191	{
192	// Timers found, so wait until it expires (or something else
193	// changes)
194	m_checkWork.waitUntil(end.second);	623✔
195	}
196	else
197	{
198	// No timers exist, so wait an arbitrary amount of time
199	m_checkWork.waitUntil(ClockT::now() + std::chrono::milliseconds(10));	739✔
200	}
201
202	// Check and execute as much work as possible, such as, all expired
203	// timers
204	checkWork();	1,362✔
205	}
206
207	// If we are shutting down, we should not have any items left,
208	// since the shutdown cancels all items
209	assert(m_items.size() == 0);	30✔
210	}	30✔
211
212	std::pair<bool, std::chrono::time_point<ClockT, DurationT>> calcWaitTime()	1,362✔
213	{
214	std::lock_guard<std::mutex> lk(m_mtx);	1,362✔
215	while(m_items.size())	1,957✔
216	{
217	if(m_items.top().handler)	1,218✔
218	{
219	// Item present, so return the new wait time
220	return std::make_pair(true, m_items.top().end);	623✔
221	}
222	else
223	{
224	// Discard empty handlers (they were cancelled)
225	m_items.pop();	595✔
226	}
227	}
228
229	// No items found, so return no wait time (causes the thread to wait
230	// indefinitely)
231	return std::make_pair(false, std::chrono::time_point<ClockT, DurationT>());	739✔
232	}	1,362✔
233
234	void checkWork()	1,362✔
235	{
236	std::unique_lock<std::mutex> lk(m_mtx);	1,362✔
237	while(m_items.size() && m_items.top().end <= ClockT::now())	2,608✔
238	{
239	WorkItem item(std::move(m_items.top()));	623✔
240	m_items.pop();	623✔
241
242	lk.unlock();	623✔
243	if(item.handler)	623✔
244	{
245	item.handler(item.id == 0);	623✔
246	}
247	lk.lock();	623✔
248	}
249	}	1,362✔
250
251	details::Semaphore m_checkWork;
252	std::thread m_thread;
253	std::atomic_bool m_finish = false;
254	uint64_t m_idcounter = 0;
255
256	struct WorkItem
257	{
258	std::chrono::time_point<ClockT, DurationT> end;
259	uint64_t id = 0; // id==0 means it was cancelled
260	std::function<void(bool)> handler;
261	bool operator>(const WorkItem& other) const	595✔
262	{
263	return end > other.end;	595✔
264	}
265	};
266
267	std::mutex m_mtx;
268	// Inheriting from priority_queue, so we can access the internal container
269	class Queue
270	: public std::priority_queue<WorkItem, std::vector<WorkItem>, std::greater<WorkItem>>
271	{
272	public:
273	std::vector<WorkItem>& getContainer()	638✔
274	{
275	return this->c;	638✔
276	}
277	} m_items;
278	};
279	} // namespace BT

BehaviorTree / BehaviorTree.CPP / 21648313606

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous