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

fix the execution order of entites in executors to favor insertion order #2537

Open
wants to merge 6 commits into
base: rolling
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 2 commits
Commits
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
143 changes: 138 additions & 5 deletions rclcpp/include/rclcpp/executors/executor_entities_collection.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <deque>
#include <functional>
#include <unordered_map>
#include <utility>
#include <vector>

#include <rclcpp/any_executable.hpp>
Expand Down Expand Up @@ -72,34 +73,47 @@ void update_entities(
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
)
{
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think here is a potential bug. If between two call of update_entities the order of elements in update_from changes, the order in update_to will not change. I am not really sure if this might happen though.

for (auto it = update_to.begin(); it != update_to.end(); ) {
for (auto it = update_to.begin_ordered(); it != update_to.end_ordered(); ) {
if (update_from.count(it->first) == 0) {
auto entity = it->second.entity.lock();
if (entity) {
on_removed(entity);
}
it = update_to.erase(it);
it = update_to.erase_ordered(it);
} else {
++it;
}
}
for (auto it = update_from.begin(); it != update_from.end(); ++it) {
for (auto it = update_from.begin_ordered(); it != update_from.end_ordered(); ++it) {
if (update_to.count(it->first) == 0) {
auto entity = it->second.entity.lock();
if (entity) {
on_added(entity);
}
update_to.insert(*it);
bool inserted = update_to.insert(*it);
Copy link
Contributor

@jmachowinski jmachowinski Jun 26, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why insert an outdated entity? I guess this should be in the if(entity) ?

// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
Comment on lines -75 to +97
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is the important part of the changes that affect the order of execution when tied, because this is what controls the order in which they are added to the wait set in the case of the wait set based executors, i.e. everything except the EventsExecutor.

}
}
}

/// A collection of entities, indexed by their corresponding handles
template<typename EntityKeyType, typename EntityValueType>
class EntityCollection
: public std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>
{
public:
/// Type of the map used for random access
using MapType = std::unordered_map<const EntityKeyType *, CollectionEntry<EntityValueType>>;

/// Type of the vector for insertion order access
// Note, we cannot use typename MapType::value_type because it makes the first
// item in the pair const, which prevents copy assignment of the pair, which
// prevents std::vector::erase from working later...
using VectorType = std::vector<std::pair<const EntityKeyType *,
CollectionEntry<EntityValueType>>>;
Comment on lines +107 to +115
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This detail was annoying, the MapType::value_type makes the first argument of the pair const (would be const EntityKeyType * const in this case), which prevents the use of the operator= on the pair, which prevents std::vector::erase from working...

Anyway, this is essentially a std::vector<typename MapType::value_type> except for that detail...

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not just store the iterator returned by std::map::insert ?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because they can be invalidated by future insertions.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I also considered storing the pointer (which is the key for the map) only, but then when iterating in order you need to do a lookup in the map for each one you want to use, which is made redundant if you just store the information twice. In fact this conclusion is what several of the "ordered dict in c++" projects I looked at online ended up using.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Because they can be invalidated by future insertions.

Nope, not for std::map :
https://en.cppreference.com/w/cpp/container/map/insert

No iterators or references are invalidated. If the insertion is successful, pointers and references to the element obtained while it is held in the node handle are invalidated, and pointers and references obtained to that element before it was extracted become valid.(since C++17)

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But the point is to use the unordered_map so that you get the better look up performance (Constant on average, worst case linear in the size of the container. for unordered_map vs Logarithmic in the size of the container. for map), and in that case I interpret it to mean that rehashing will invalidate them:

If after the operation the new number of elements is greater than old max_load_factor() * bucket_count() a rehashing takes place.
If rehashing occurs (due to the insertion), all iterators are invalidated. Otherwise (no rehashing), iterators are not invalidated. If the insertion is successful, pointers and references to the element obtained while it is held in the node handle are invalidated, and pointers and references obtained to that element before it was extracted become valid.(since C++17)

-- https://en.cppreference.com/w/cpp/container/unordered_map/insert

Which is also the conclusion of this SO answer (which I was using to try and confirm my interpretation): https://stackoverflow.com/a/54004916


For what it's worth, I considered using std::map and its iterators too.
However, the conclusion I came to is that whether it's a vector of iterators (pointers) or a vector of triplets of pointers, the impact is not much different on memory, even at relatively large scales. Instead, I thought preserving the cpu time optimizations was better for the executor, given the feedback we've consistently gotten, but that's a generalization.


/// Key type of the map
using Key = const EntityKeyType *;

Expand All @@ -125,6 +139,125 @@ class EntityCollection
{
update_entities(other, *this, on_added, on_removed);
}

// Below are some forwarded functions to the map and vector as appropriate.

typename MapType::size_type count(const Key & key) const
{
return map_.count(key);
}

typename MapType::iterator begin()
{
return map_.begin();
}

typename MapType::const_iterator begin() const
{
return map_.begin();
}

typename MapType::iterator end()
{
return map_.end();
}

typename MapType::const_iterator end() const
{
return map_.end();
}

typename VectorType::iterator begin_ordered()
{
return insertion_order_.begin();
}

typename VectorType::const_iterator begin_ordered() const
{
return insertion_order_.begin();
}

typename VectorType::iterator end_ordered()
{
return insertion_order_.end();
}

typename VectorType::const_iterator end_ordered() const
{
return insertion_order_.end();
}

typename MapType::const_iterator find(const Key & key) const
{
return map_.find(key);
}

bool empty() const noexcept
{
return insertion_order_.empty();
}

typename VectorType::size_type size() const noexcept
{
return insertion_order_.size();
}

typename MapType::iterator erase(typename MapType::const_iterator pos)
{
// from: https://en.cppreference.com/w/cpp/container/unordered_map/erase
// The iterator pos must be valid and dereferenceable.
// Thus the end() iterator (which is valid, but is not dereferenceable)
// cannot be used as a value for pos.
//
// Therefore we can use pos-> here safely.
insertion_order_.erase(
std::remove_if(
insertion_order_.begin(),
insertion_order_.end(),
[&pos](auto value) {
return value.first == pos->first;
}));
return map_.erase(pos);
}

typename VectorType::iterator erase_ordered(typename VectorType::const_iterator pos)
{
// from: https://en.cppreference.com/w/cpp/container/vector/erase
// The iterator pos must be valid and dereferenceable. Thus the end
// () iterator (which is valid, but is not dereferenceable) cannot be used
// as a value for pos.
//
// Therefore we can use pos-> here safely.

assert(map_.erase(pos->first) == 1);
return insertion_order_.erase(pos);
}

void clear() noexcept
{
insertion_order_.clear();
return map_.clear();
}

/// Insert into the collection and return true if inserted, otherwise false
/**
* Insertion will fail, and return false, if attempting to insert a duplicate
* entity.
*/
[[nodiscard]]
bool insert(typename MapType::value_type value)
{
if (!map_.insert(value).second) {
// attempting to insert a duplicate entity
return false;
}
insertion_order_.push_back(value);
return true;
}

private:
MapType map_;
VectorType insertion_order_;
wjwwood marked this conversation as resolved.
Show resolved Hide resolved
};

/// Represent the total set of entities for a single executor
Expand Down
6 changes: 5 additions & 1 deletion rclcpp/src/rclcpp/executor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -673,7 +673,11 @@ Executor::collect_entities()
current_notify_waitable_ = std::make_shared<rclcpp::executors::ExecutorNotifyWaitable>(
*notify_waitable_);
auto notify_waitable = std::static_pointer_cast<rclcpp::Waitable>(current_notify_waitable_);
collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}});
bool inserted = collection.waitables.insert({notify_waitable.get(), {notify_waitable, {}}});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
wjwwood marked this conversation as resolved.
Show resolved Hide resolved
}

// We must remove expired entities here, so that we don't continue to use older entities.
Expand Down
34 changes: 27 additions & 7 deletions rclcpp/src/rclcpp/executors/executor_entities_collection.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -43,10 +43,10 @@ size_t ExecutorEntitiesCollection::remove_expired_entities()
{
auto remove_entities = [](auto & collection) -> size_t {
size_t removed = 0;
for (auto it = collection.begin(); it != collection.end(); ) {
for (auto it = collection.begin_ordered(); it != collection.end_ordered(); ) {
if (it->second.entity.expired()) {
++removed;
it = collection.erase(it);
it = collection.erase_ordered(it);
Comment on lines 44 to +49
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This change was not strictly necessary, but I reasoned it was slightly faster to provide the iterator for erasure to the vector and look up the iterator for the map, rather than the other way around, which is what erase_ordered does.

} else {
++it;
}
Expand Down Expand Up @@ -79,39 +79,59 @@ build_entities_collection(
if (group_ptr->can_be_taken_from().load()) {
group_ptr->collect_all_ptrs(
[&collection, weak_group_ptr](const rclcpp::SubscriptionBase::SharedPtr & subscription) {
collection.subscriptions.insert(
bool inserted = collection.subscriptions.insert(
{
subscription->get_subscription_handle().get(),
{subscription, weak_group_ptr}
});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
},
[&collection, weak_group_ptr](const rclcpp::ServiceBase::SharedPtr & service) {
collection.services.insert(
bool inserted = collection.services.insert(
{
service->get_service_handle().get(),
{service, weak_group_ptr}
});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
},
[&collection, weak_group_ptr](const rclcpp::ClientBase::SharedPtr & client) {
collection.clients.insert(
bool inserted = collection.clients.insert(
{
client->get_client_handle().get(),
{client, weak_group_ptr}
});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
},
[&collection, weak_group_ptr](const rclcpp::TimerBase::SharedPtr & timer) {
collection.timers.insert(
bool inserted = collection.timers.insert(
{
timer->get_timer_handle().get(),
{timer, weak_group_ptr}
});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
},
[&collection, weak_group_ptr](const rclcpp::Waitable::SharedPtr & waitable) {
collection.waitables.insert(
bool inserted = collection.waitables.insert(
{
waitable.get(),
{waitable, weak_group_ptr}
});
// Should never be false, so this is a defensive check, mark unused too
// in order to avoid a warning in release builds.
assert(inserted);
RCUTILS_UNUSED(inserted);
}
);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -511,9 +511,19 @@ EventsExecutor::add_notify_waitable_to_collection(
{
// The notify waitable is not associated to any group, so use an invalid one
rclcpp::CallbackGroup::WeakPtr weak_group_ptr;
collection.insert(
bool inserted = collection.insert(
{
this->notify_waitable_.get(),
{this->notify_waitable_, weak_group_ptr}
});
// Explicitly ignore if the notify waitable was not inserted because that means
// it was already inserted, which happens initially as it is explicitly added
// in the constructor as well as every time the collection is reset, so on
// the first reset there is a second insertion attempt.
// We could check before trying to insert, but that would require a "find" call
// on each refresh, which is expensive, and otherwise it would require additional
// state in this class to detect the initial case where it is added twice.
// Therefore we just insert and ignore it if it fails (the only way it fails
// is when a duplicate is inserted).
RCUTILS_UNUSED(inserted);
Comment on lines -514 to +528
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is an oddity I found with the events executor, where it was always inserting the notify waitable twice on one occasion (first refresh after construction), and it wasn't checking the return value of insert, which if we had checked would have occasionally been false meaning it wasn't inserted. This didn't end up mattering, but it did feel weird and broke some things when I added the vector storage along side the map.

Anyway I landed on this approach where we continue to add it twice sometimes, but we just ignore it when it wasn't inserted as the fact that it has been inserted is important, not the order in which it was, at least in the specific case of this notify waitable.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think that this was a bug.
I opened a PR to fix it #2564

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Cool, thanks for checking up on it. I guess I'll wait for that pr to merge and then update this. I have to check into timer execution order on Windows for these new tests anyways.

}
Loading