Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Utilize rclcpp::WaitSet as part of the executors #2142

Merged
merged 106 commits into from
Mar 29, 2024
Merged
Changes from 1 commit
Commits
Show all changes
106 commits
Select commit Hold shift + click to select a range
2bf88de
Deprecate callback_group call taking context
mjcarroll Mar 29, 2023
9099635
Add base executor objects that can be used by implementors
mjcarroll Mar 29, 2023
2426056
Template common operations
mjcarroll Mar 29, 2023
173ffd6
Address reviewer feedback:
mjcarroll Mar 29, 2023
a524bf0
Lint
mjcarroll Mar 29, 2023
89f2106
Address reviewer feedback and fix templates
mjcarroll Mar 30, 2023
9695eaa
Merge branch 'rolling' into mjcarroll/executor_structures
mjcarroll Mar 30, 2023
e173e5a
Lint and docs
mjcarroll Mar 30, 2023
653d1a3
Make executor own the notify waitable
mjcarroll Mar 31, 2023
a6c4c1b
Add pending queue to collector, remove from waitable
mjcarroll Apr 3, 2023
9dd48ce
Change interrupt guard condition to shared_ptr
mjcarroll Apr 3, 2023
6267741
Lint and docs
mjcarroll Apr 3, 2023
974e845
Utilize rclcpp::WaitSet as part of the executors
mjcarroll Mar 29, 2023
1b1a915
Don't exchange atomic twice
mjcarroll Apr 3, 2023
0a9c9a6
Fix add_node and add more tests
mjcarroll Apr 3, 2023
0ae0bea
Make get_notify_guard_condition follow API tick-tock
mjcarroll Apr 3, 2023
87f41bf
Improve callback group tick-tocking
mjcarroll Apr 3, 2023
5809328
Don't lock twice
mjcarroll Apr 3, 2023
debe396
Address reviewer feedback
mjcarroll Apr 4, 2023
c4b6589
Add thread safety annotations and make locks consistent
mjcarroll Apr 4, 2023
8782fff
Merge branch 'mjcarroll/executor_structures' into mjcarroll/rclcpp_wa…
mjcarroll Apr 4, 2023
0c912b6
@wip
mjcarroll Apr 4, 2023
ae9a845
Reset callback groups for multithreaded executor
mjcarroll Apr 6, 2023
3db897a
Avoid many small function calls when building executables
mjcarroll Apr 6, 2023
20d3cca
Re-trigger guard condition if buffer has data
mjcarroll Apr 6, 2023
cd7aaba
Address reviewer feedback
mjcarroll Apr 11, 2023
d8ff831
Trace points
mjcarroll Apr 11, 2023
e52b242
Remove tracepoints
mjcarroll Apr 11, 2023
0c3c899
Reducing diff
mjcarroll Apr 11, 2023
d2d271b
Reduce diff
mjcarroll Apr 11, 2023
200f733
Uncrustify
mjcarroll Apr 11, 2023
985c1f4
Restore tests
mjcarroll Apr 11, 2023
03471fc
Back to weak_ptr and reduce test time
mjcarroll Apr 11, 2023
5c70cb6
reduce diff and lint
mjcarroll Apr 11, 2023
31d25fc
Restore static single threaded tests that weren't working before
mjcarroll Apr 11, 2023
38c80fd
Merge branch 'mjcarroll/executor_structures' into mjcarroll/rclcpp_wa…
mjcarroll Apr 11, 2023
7a81a8f
Restore more tests
mjcarroll Apr 11, 2023
38387e0
Fix multithreaded test
mjcarroll Apr 11, 2023
a2f3977
Fix assert
mjcarroll Apr 11, 2023
1ad6ad6
Fix constructor test
mjcarroll Apr 12, 2023
cd56124
Change ready_executables signature back
mjcarroll Apr 12, 2023
3a80b86
Don't enforce removing callback groups before nodes
mjcarroll Apr 12, 2023
6379f0c
Remove the "add_valid_node" API
mjcarroll Apr 12, 2023
4b2e280
Merge branch 'mjcarroll/executor_structures' into mjcarroll/rclcpp_wa…
mjcarroll Apr 12, 2023
855c64d
Only notify if the trigger condition is valid
mjcarroll Apr 12, 2023
d9a9206
Only trigger if valid and needed
mjcarroll Apr 13, 2023
43c8f45
Merge branch 'mjcarroll/executor_structures' into mjcarroll/rclcpp_wa…
mjcarroll Apr 13, 2023
fcc33e9
Fix spin_some/spin_all implementation
mjcarroll Apr 13, 2023
64cba3b
Restore single threaded executor
mjcarroll Apr 13, 2023
49962fd
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Apr 13, 2023
838d1ae
Merge branch 'rolling' into mjcarroll/executor_structures
mjcarroll Apr 13, 2023
2c3a36c
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Apr 13, 2023
ab3bbf4
Merge branch 'rolling' into mjcarroll/executor_structures
mjcarroll Apr 13, 2023
039d2b1
Merge branch 'mjcarroll/executor_structures' into mjcarroll/rclcpp_wa…
mjcarroll Apr 13, 2023
ffdb562
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Apr 17, 2023
80077dd
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Apr 17, 2023
ad5931b
Picking ABI-incompatible executor changes
mjcarroll Apr 17, 2023
e3f692b
Merge branch 'mjcarroll/rclcpp_waitset_executor_abi_only' into mjcarr…
mjcarroll Apr 17, 2023
acfc0e2
Add PIMPL
mjcarroll Apr 17, 2023
aff46a4
Merge branch 'mjcarroll/rclcpp_waitset_executor_abi_only' into mjcarr…
mjcarroll Apr 17, 2023
c6612ec
Additional waitset prune
mjcarroll Apr 18, 2023
43d6100
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Apr 24, 2023
670843a
Fix bad merge
mjcarroll Apr 25, 2023
e364d89
Expand test timeout
mjcarroll Apr 27, 2023
d63d677
Introduce method to clear expired entities from a collection
mjcarroll May 2, 2023
8c2ed20
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jun 5, 2023
c8cc2c5
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jun 9, 2023
9ed1cc3
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jun 9, 2023
77d2810
Merge remote-tracking branch 'origin/mjcarroll/rclcpp_waitset_executo…
mjcarroll Jun 14, 2023
9ee5e26
Merge branch 'rolling' into rclcpp_waitset_executor
mjcarroll Jun 20, 2023
b9e87d2
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jul 10, 2023
2a4e932
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jul 18, 2023
66ba3b0
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Oct 25, 2023
3a51869
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Nov 14, 2023
fbae914
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
clalancette Nov 27, 2023
4fe9f68
Make sure to call remove_expired_entities().
clalancette Nov 29, 2023
eea79ad
Prune queued work when callback group is removed
mjcarroll Nov 30, 2023
37550f8
Prune subscriptions from dynamic storage
mjcarroll Nov 30, 2023
760c8fe
Styles fixes.
clalancette Nov 30, 2023
af43e2a
Re-trigger guard conditions
mjcarroll Dec 5, 2023
cfb7e79
Merge remote-tracking branch 'origin/mjcarroll/rclcpp_waitset_executo…
mjcarroll Dec 5, 2023
4ce1645
Condense to just use watiable.take_data
mjcarroll Dec 5, 2023
662f440
Lint
mjcarroll Dec 6, 2023
02d9cd6
Address reviewer comments (nits)
mjcarroll Dec 8, 2023
8ed094e
Lock mutex when copying
mjcarroll Dec 8, 2023
fe4333f
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jan 11, 2024
1376bd7
Refactors to static single threaded based on reviewers
mjcarroll Jan 11, 2024
6208333
More small refactoring
mjcarroll Jan 11, 2024
6b671aa
Lint
mjcarroll Jan 23, 2024
7349408
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jan 23, 2024
527d284
Lint
mjcarroll Jan 23, 2024
ed12cdb
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Jan 26, 2024
92aff2c
Add ready executable accessors to WaitResult
mjcarroll Jan 26, 2024
bf224d6
Make use of accessors from wait_set
mjcarroll Jan 30, 2024
ddac821
Fix tests
mjcarroll Feb 12, 2024
c984469
Merge branch 'rolling' into mjcarroll/rclcpp_waitset_executor
mjcarroll Feb 12, 2024
1a9784d
Fix more tests
mjcarroll Feb 13, 2024
892f70a
Tidy up single threaded executor implementation
mjcarroll Feb 13, 2024
d254d0c
Don't null out timer, rely on call
mjcarroll Feb 27, 2024
62a3f55
Merge remote-tracking branch 'origin/rolling' into mjcarroll/rclcpp_w…
wjwwood Mar 4, 2024
5769359
change how timers are checked from wait result in executors
wjwwood Mar 19, 2024
15f321f
peak -> peek
wjwwood Mar 20, 2024
14c882d
Merge remote-tracking branch 'origin/rolling' into mjcarroll/rclcpp_w…
wjwwood Mar 22, 2024
836946e
fix bug in next_waitable logic
wjwwood Mar 26, 2024
2a88295
fix bug in StaticSTE that broke the add callback groups to executor t…
wjwwood Mar 26, 2024
245bb50
style
wjwwood Mar 26, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
change how timers are checked from wait result in executors
Signed-off-by: William Woodall <william@osrfoundation.org>
wjwwood committed Mar 19, 2024
commit 57693595edd9a99a795d8f55f3c0d728817ac648
6 changes: 4 additions & 2 deletions rclcpp/include/rclcpp/callback_group.hpp
Original file line number Diff line number Diff line change
@@ -184,7 +184,8 @@ class CallbackGroup
* \return the number of entities in the callback group.
*/
RCLCPP_PUBLIC
size_t size() const;
size_t
size() const;

/// Return a reference to the 'can be taken' atomic boolean.
/**
@@ -216,7 +217,8 @@ class CallbackGroup
* \param[in] waitable_fuinc Function to execute for each waitable
*/
RCLCPP_PUBLIC
void collect_all_ptrs(
void
collect_all_ptrs(
std::function<void(const rclcpp::SubscriptionBase::SharedPtr &)> sub_func,
std::function<void(const rclcpp::ServiceBase::SharedPtr &)> service_func,
std::function<void(const rclcpp::ClientBase::SharedPtr &)> client_func,
119 changes: 90 additions & 29 deletions rclcpp/include/rclcpp/wait_result.hpp
Original file line number Diff line number Diff line change
@@ -20,12 +20,16 @@
#include <iostream>
#include <memory>
#include <stdexcept>
#include <utility>

#include "rcl/wait.h"

#include "rclcpp/macros.hpp"
#include "rclcpp/wait_result_kind.hpp"

#include "rclcpp/client.hpp"
#include "rclcpp/service.hpp"
#include "rclcpp/subscription_base.hpp"
#include "rclcpp/timer.hpp"

namespace rclcpp
@@ -138,33 +142,80 @@ class WaitResult final
}
}

std::shared_ptr<rclcpp::TimerBase> next_ready_timer()
/// Get the next ready timer and its index in the wait result, but do not clear it.
/**
* The returned timer is not cleared automatically, as it the case with the
* other next_ready_*()-like functions.
* Instead, this function returns the timer and the index that identifies it
* in the wait result, so that it can be cleared (marked as taken or used)
* in a separate step with clear_timer_with_index().
* This is necessary in some multi-threaded executor implementations.
*
* If the timer is not cleared using the index, subsequent calls to this
* function will return the same timer.
*
* If there is no ready timer, then nullptr will be returned and the index
* will be invalid and should not be used.
*
* \param[in] start_index index at which to start searching for the next ready
* timer in the wait result. If the start_index is out of bounds for the
* list of timers in the wait result, then {nullptr, start_index} will be
* returned. Defaults to 0.
* \return next ready timer pointer and its index in the wait result, or
* {nullptr, start_index} if none was found.
*/
std::pair<std::shared_ptr<rclcpp::TimerBase>, size_t>
peak_next_ready_timer(size_t start_index = 0)
{
check_wait_result_dirty();
auto ret = std::shared_ptr<rclcpp::TimerBase>{nullptr};
size_t ii = start_index;
if (this->kind() == WaitResultKind::Ready) {
auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set_pointer_->size_of_timers(); ++ii) {
if (rcl_wait_set.timers[ii] != nullptr &&
wait_set_pointer_->timers(ii)->call())
{
ret = wait_set_pointer_->timers(ii);
auto & wait_set = this->get_wait_set();
auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
for (; ii < wait_set.size_of_timers(); ++ii) {
if (rcl_wait_set.timers[ii] != nullptr) {
ret = wait_set.timers(ii);
break;
}
}
}
return ret;
return {ret, ii};
}

/// Clear the timer at the given index.
/**
* Clearing a timer from the wait result prevents it from being returned by
* the peak_next_ready_timer() on subsequent calls.
*
* The index should come from the peak_next_ready_timer() function, and
* should only be used with this function if the timer pointer was valid.
*
* \throws std::out_of_range if the given index is out of range
*/
void
clear_timer_with_index(size_t index)
{
auto & wait_set = this->get_wait_set();
auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
if (index >= wait_set.size_of_timers()) {
throw std::out_of_range("given timer index is out of range");
}
rcl_wait_set.timers[index] = nullptr;
}

std::shared_ptr<rclcpp::SubscriptionBase> next_ready_subscription()
/// Get the next ready subscription, clearing it from the wait result.
std::shared_ptr<rclcpp::SubscriptionBase>
next_ready_subscription()
{
check_wait_result_dirty();
auto ret = std::shared_ptr<rclcpp::SubscriptionBase>{nullptr};
if (this->kind() == WaitResultKind::Ready) {
auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set_pointer_->size_of_subscriptions(); ++ii) {
auto & wait_set = this->get_wait_set();
auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set.size_of_subscriptions(); ++ii) {
if (rcl_wait_set.subscriptions[ii] != nullptr) {
ret = wait_set_pointer_->subscriptions(ii);
ret = wait_set.subscriptions(ii);
rcl_wait_set.subscriptions[ii] = nullptr;
break;
}
@@ -173,15 +224,18 @@ class WaitResult final
return ret;
}

std::shared_ptr<rclcpp::ServiceBase> next_ready_service()
/// Get the next ready service, clearing it from the wait result.
std::shared_ptr<rclcpp::ServiceBase>
next_ready_service()
{
check_wait_result_dirty();
auto ret = std::shared_ptr<rclcpp::ServiceBase>{nullptr};
if (this->kind() == WaitResultKind::Ready) {
auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set_pointer_->size_of_services(); ++ii) {
auto & wait_set = this->get_wait_set();
auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set.size_of_services(); ++ii) {
if (rcl_wait_set.services[ii] != nullptr) {
ret = wait_set_pointer_->services(ii);
ret = wait_set.services(ii);
rcl_wait_set.services[ii] = nullptr;
break;
}
@@ -190,16 +244,18 @@ class WaitResult final
return ret;
}

std::shared_ptr<rclcpp::ClientBase> next_ready_client()
/// Get the next ready client, clearing it from the wait result.
std::shared_ptr<rclcpp::ClientBase>
next_ready_client()
{
check_wait_result_dirty();
auto ret = std::shared_ptr<rclcpp::ClientBase>{nullptr};
if (this->kind() == WaitResultKind::Ready) {
auto & rcl_wait_set = wait_set_pointer_->storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set_pointer_->size_of_clients(); ++ii) {
auto & wait_set = this->get_wait_set();
auto & rcl_wait_set = wait_set.storage_get_rcl_wait_set();
for (size_t ii = 0; ii < wait_set.size_of_clients(); ++ii) {
if (rcl_wait_set.clients[ii] != nullptr) {
ret = wait_set_pointer_->clients(ii);

ret = wait_set.clients(ii);
rcl_wait_set.clients[ii] = nullptr;
break;
}
@@ -208,16 +264,19 @@ class WaitResult final
return ret;
}

std::shared_ptr<rclcpp::Waitable> next_ready_waitable()
/// Get the next ready waitable, clearing it from the wait result.
std::shared_ptr<rclcpp::Waitable>
next_ready_waitable()
{
check_wait_result_dirty();
auto waitable = std::shared_ptr<rclcpp::Waitable>{nullptr};
auto data = std::shared_ptr<void>{nullptr};

if (this->kind() == WaitResultKind::Ready) {
auto rcl_wait_set = get_wait_set().get_rcl_wait_set();
while (next_waitable_index_ < wait_set_pointer_->size_of_waitables()) {
auto cur_waitable = wait_set_pointer_->waitables(next_waitable_index_);
auto & wait_set = this->get_wait_set();
auto rcl_wait_set = wait_set.get_rcl_wait_set();
while (next_waitable_index_ < wait_set.size_of_waitables()) {
auto cur_waitable = wait_set.waitables(next_waitable_index_);
if (cur_waitable != nullptr && cur_waitable->is_ready(&rcl_wait_set)) {
waitable = cur_waitable;
}
@@ -245,14 +304,16 @@ class WaitResult final
// Should be enforced by the static factory methods on this class.
assert(WaitResultKind::Ready == wait_result_kind);
// Secure thread-safety (if provided) and shared ownership (if needed).
wait_set_pointer_->wait_result_acquire();
this->get_wait_set().wait_result_acquire();
}

void check_wait_result_dirty()
/// Check if the wait result is invalid because the wait set was modified.
void
check_wait_result_dirty()
{
// In the case that the waitset was modified while the result was out,
// In the case that the wait set was modified while the result was out,
// we must mark the wait result as no longer valid
if (wait_set_pointer_ && wait_set_pointer_->wait_result_dirty_) {
if (wait_set_pointer_ && this->get_wait_set().wait_result_dirty_) {
this->wait_result_kind_ = WaitResultKind::Invalid;
}
}
20 changes: 17 additions & 3 deletions rclcpp/src/rclcpp/executor.cpp
Original file line number Diff line number Diff line change
@@ -559,8 +559,7 @@ Executor::execute_service(rclcpp::ServiceBase::SharedPtr service)
}

void
Executor::execute_client(
rclcpp::ClientBase::SharedPtr client)
Executor::execute_client(rclcpp::ClientBase::SharedPtr client)
{
auto request_header = client->create_request_header();
std::shared_ptr<void> response = client->create_response();
@@ -674,13 +673,28 @@ Executor::get_next_ready_executable(AnyExecutable & any_executable)
}

if (!valid_executable) {
while (auto timer = wait_result_->next_ready_timer()) {
size_t current_timer_index = 0;
while (true) {
auto [timer, timer_index] = wait_result_->peak_next_ready_timer(current_timer_index);
if (nullptr == timer) {
break;
}
current_timer_index = timer_index;
auto entity_iter = current_collection_.timers.find(timer->get_timer_handle().get());
if (entity_iter != current_collection_.timers.end()) {
auto callback_group = entity_iter->second.callback_group.lock();
if (callback_group && !callback_group->can_be_taken_from()) {
continue;
}
// At this point the timer is either ready for execution or was perhaps
// it was canceled, based on the result of call(), but either way it
// should not be checked again from peak_next_ready_timer(), so clear
// it from the wait result.
wait_result_->clear_timer_with_index(current_timer_index);
// Check that the timer should be called still, i.e. it wasn't canceled.
if (!timer->call()) {
continue;
}
any_executable.timer = timer;
any_executable.callback_group = callback_group;
valid_executable = true;
28 changes: 11 additions & 17 deletions rclcpp/src/rclcpp/executors/static_single_threaded_executor.cpp
Original file line number Diff line number Diff line change
@@ -40,21 +40,7 @@ StaticSingleThreadedExecutor::spin()
// except we need to keep the wait result to reproduce the StaticSingleThreadedExecutor
// behavior.
while (rclcpp::ok(this->context_) && spinning.load()) {
std::deque<rclcpp::AnyExecutable> to_exec;

std::lock_guard<std::mutex> guard(mutex_);
if (current_collection_.empty() || this->entities_need_rebuild_.load()) {
this->collect_entities();
}

auto wait_result = wait_set_.wait(std::chrono::nanoseconds(-1));
if (wait_result.kind() == WaitResultKind::Empty) {
RCUTILS_LOG_WARN_NAMED(
"rclcpp",
"empty wait set received in wait(). This should never happen.");
continue;
}
execute_ready_executables(current_collection_, wait_result, false);
this->spin_once_impl(std::chrono::nanoseconds(-1));
}
}

@@ -101,7 +87,8 @@ StaticSingleThreadedExecutor::spin_some_impl(std::chrono::nanoseconds max_durati
// Execute ready executables
bool work_available = this->execute_ready_executables(
current_collection_,
wait_result.value(), false);
wait_result.value(),
false);
if (!work_available || !exhaustive) {
break;
}
@@ -158,9 +145,16 @@ bool StaticSingleThreadedExecutor::execute_ready_executables(
}
}

while (auto timer = wait_result.next_ready_timer()) {
size_t current_timer_index = 0;
while (true) {
auto [timer, timer_index] = wait_result.peak_next_ready_timer(current_timer_index);
if (nullptr == timer) {
break;
}
current_timer_index = timer_index;
auto entity_iter = collection.timers.find(timer->get_timer_handle().get());
if (entity_iter != collection.timers.end()) {
wait_result.clear_timer_with_index(current_timer_index);
if (timer->call()) {
execute_timer(timer);
any_ready_executable = true;
21 changes: 15 additions & 6 deletions rclcpp/test/rclcpp/executors/test_executors.cpp
Original file line number Diff line number Diff line change
@@ -172,17 +172,17 @@ TYPED_TEST(TestExecutors, spinWhileAlreadySpinning)
{
using ExecutorType = TypeParam;
ExecutorType executor;
executor.add_node(this->node);

std::atomic_bool timer_completed = false;
auto timer = this->node->create_wall_timer(
1ms, [&]() {
timer_completed.store(true);
});

executor.add_node(this->node);
std::thread spinner([&]() {executor.spin();});
// Sleep for a short time to verify executor.spin() is going, and didn't throw.

// Sleep for a short time to verify executor.spin() is going, and didn't throw.
auto start = std::chrono::steady_clock::now();
while (!timer_completed.load() && (std::chrono::steady_clock::now() - start) < 10s) {
std::this_thread::sleep_for(1ms);
@@ -339,11 +339,18 @@ class TestWaitable : public rclcpp::Waitable
void
add_to_wait_set(rcl_wait_set_t * wait_set) override
{
if (trigger_count_ > 0) {
// Keep the gc triggered until the trigger count is reduced back to zero.
// This is necessary if trigger() results in the wait set waking, but not
// executing this waitable, in which case it needs to be re-triggered.
gc_.trigger();
}
rclcpp::detail::add_guard_condition_to_rcl_wait_set(*wait_set, gc_);
}

void trigger()
{
trigger_count_++;
gc_.trigger();
}

@@ -371,6 +378,7 @@ class TestWaitable : public rclcpp::Waitable
execute(std::shared_ptr<void> & data) override
{
(void) data;
trigger_count_--;
count_++;
std::this_thread::sleep_for(3ms);
}
@@ -400,6 +408,7 @@ class TestWaitable : public rclcpp::Waitable
}

private:
std::atomic<size_t> trigger_count_ = 0;
std::atomic<size_t> count_ = 0;
rclcpp::GuardCondition gc_;
};
@@ -461,16 +470,16 @@ TYPED_TEST(TestExecutors, spinSome)
std::thread spinner([&spin_exited, &executor, this]() {
executor.spin_some(1s);
executor.remove_node(this->node, true);
spin_exited = true;
spin_exited.store(true);
});

// Do some work until sufficient calls to the waitable occur, but keep going until either
// count becomes too large, spin exits, or the 1 second timeout completes.
auto start = std::chrono::steady_clock::now();
while (
my_waitable->get_count() <= 1 &&
!spin_exited &&
(std::chrono::steady_clock::now() - start < 1s))
!spin_exited.load() &&
std::chrono::steady_clock::now() - start < 10s)
{
my_waitable->trigger();
this->publisher->publish(test_msgs::msg::Empty());
@@ -480,7 +489,7 @@ TYPED_TEST(TestExecutors, spinSome)
// the first iteration of the while loop
EXPECT_LE(1u, my_waitable->get_count());
waitable_interfaces->remove_waitable(my_waitable, nullptr);
EXPECT_TRUE(spin_exited);
EXPECT_TRUE(spin_exited.load());
// Cancel if it hasn't exited already.
executor.cancel();

Loading