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
Lint
Signed-off-by: Michael Carroll <mjcarroll@intrinsic.ai>
mjcarroll committed Mar 29, 2023
commit a524bf016ad3c8f7a557d5de5eec44108fe2f3f9
25 changes: 13 additions & 12 deletions rclcpp/include/rclcpp/executors/executor_entities_collection.hpp
Original file line number Diff line number Diff line change
@@ -15,7 +15,6 @@
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_

#include "rclcpp/subscription_base.hpp"
#include <deque>
#include <unordered_map>
#include <vector>
@@ -33,18 +32,19 @@ namespace rclcpp
namespace executors
{

template <typename EntityValueType>
struct CollectionEntry {
template<typename EntityValueType>
struct CollectionEntry
{
typename EntityValueType::WeakPtr entity;
rclcpp::CallbackGroup::WeakPtr callback_group;
};

template <typename CollectionType>
template<typename CollectionType>
void update_entities(
const CollectionType & update_from,
CollectionType update_to,
std::function<void (typename CollectionType::mapped_type::EntitySharedPtr) > on_added,
std::function<void (typename CollectionType::mapped_type::EntitySharedPtr) > on_removed
std::function<void(typename CollectionType::mapped_type::EntitySharedPtr)> on_added,
std::function<void(typename CollectionType::mapped_type::EntitySharedPtr)> on_removed
)
{
for (auto it = update_to.begin(); it != update_to.end(); ) {
@@ -68,18 +68,19 @@ void update_entities(
}
}
}
template <typename EntityKeyType, typename EntityValueType>
class EntityCollection:
public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
template<typename EntityKeyType, typename EntityValueType>
class EntityCollection
: public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
{
public:
using Key = const EntityKeyType *;
using EntityWeakPtr = typename EntityValueType::WeakPtr;
using EntitySharedPtr = typename EntityValueType::SharedPtr;

void update(const EntityCollection<EntityKeyType, EntityValueType> & other,
std::function<void (EntitySharedPtr)> on_added,
std::function<void (EntitySharedPtr)> on_removed)
void update(
const EntityCollection<EntityKeyType, EntityValueType> & other,
std::function<void(EntitySharedPtr)> on_added,
std::function<void(EntitySharedPtr)> on_removed)
{
update_entities(*this, other, on_added, on_removed);
}
Original file line number Diff line number Diff line change
@@ -63,7 +63,7 @@ class ExecutorEntitiesCollector
*/
RCLCPP_PUBLIC
explicit ExecutorEntitiesCollector(
std::function<void(void)> on_notify_waitable_callback = {});
std::function<void(void)> on_notify_waitable_callback = {});

/// Destructor
RCLCPP_PUBLIC
@@ -172,7 +172,8 @@ class ExecutorEntitiesCollector
RCLCPP_PUBLIC
void
add_automatically_associated_callback_groups(
std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> nodes_to_check) RCPPUTILS_TSA_REQUIRES(mutex_);
std::list<rclcpp::node_interfaces::NodeBaseInterface::WeakPtr> nodes_to_check)
RCPPUTILS_TSA_REQUIRES(mutex_);

RCLCPP_PUBLIC
void
13 changes: 7 additions & 6 deletions rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
Original file line number Diff line number Diff line change
@@ -82,13 +82,14 @@ build_entities_collection(
}
}

template <typename EntityCollectionType>
template<typename EntityCollectionType>
void check_ready(
EntityCollectionType & collection,
std::deque<rclcpp::AnyExecutable> & executables,
size_t size_of_waited_entities,
typename EntityCollectionType::Key * waited_entities,
std::function<bool(rclcpp::AnyExecutable, typename EntityCollectionType::EntitySharedPtr)> fill_executable)
std::function<bool(rclcpp::AnyExecutable,
typename EntityCollectionType::EntitySharedPtr)> fill_executable)
{
for (size_t ii = 0; ii < size_of_waited_entities; ++ii) {
if (waited_entities[ii]) {
@@ -130,7 +131,7 @@ ready_executables(
ret,
rcl_wait_set.size_of_timers,
rcl_wait_set.timers,
[](auto exec, auto timer){
[](auto exec, auto timer) {
if (!timer->call()) {
return false;
}
@@ -143,7 +144,7 @@ ready_executables(
ret,
rcl_wait_set.size_of_subscriptions,
rcl_wait_set.subscriptions,
[](auto exec, auto subscription){
[](auto exec, auto subscription) {
exec.subscription = subscription;
return true;
});
@@ -153,7 +154,7 @@ ready_executables(
ret,
rcl_wait_set.size_of_services,
rcl_wait_set.services,
[](auto exec, auto service){
[](auto exec, auto service) {
exec.service = service;
return true;
});
@@ -163,7 +164,7 @@ ready_executables(
ret,
rcl_wait_set.size_of_clients,
rcl_wait_set.clients,
[](auto exec, auto client){
[](auto exec, auto client) {
exec.client = client;
return true;
});
7 changes: 3 additions & 4 deletions rclcpp/src/rclcpp/executors/executor_entities_collector.cpp
Original file line number Diff line number Diff line change
@@ -24,10 +24,9 @@ namespace executors
{

ExecutorEntitiesCollector::ExecutorEntitiesCollector(
std::function<void(void)> on_notify_waitable_callback) {
notify_waitable_ = std::make_shared<ExecutorNotifyWaitable>(
[on_notify_waitable_callback](){on_notify_waitable_callback();});

std::function<void(void)> on_notify_waitable_callback)
: notify_waitable_(std::make_shared<ExecutorNotifyWaitable>(on_notify_waitable_callback))
{
}

ExecutorEntitiesCollector::~ExecutorEntitiesCollector()