Program Listing for File executor_entities_collection.hpp
↰ Return to documentation for file (include/rclcpp/executors/executor_entities_collection.hpp
)
// Copyright 2023 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
#define RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_
#include <deque>
#include <functional>
#include <unordered_map>
#include <vector>
#include <rclcpp/any_executable.hpp>
#include <rclcpp/node_interfaces/node_base.hpp>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/executors/executor_notify_waitable.hpp>
#include <rclcpp/visibility_control.hpp>
#include <rclcpp/wait_result.hpp>
#include <rclcpp/wait_set.hpp>
namespace rclcpp
{
namespace executors
{
template<typename EntityValueType>
struct CollectionEntry
{
using EntityWeakPtr = typename EntityValueType::WeakPtr;
using EntitySharedPtr = typename EntityValueType::SharedPtr;
EntityWeakPtr entity;
rclcpp::CallbackGroup::WeakPtr callback_group;
};
/*
* Iterates update_from and update_to to see which entities have been added/removed between
* the two collections.
*
* For each new entry (in update_from, but not in update_to),
* add the entity and fire the on_added callback
* For each removed entry (in update_to, but not in update_from),
* remove the entity and fire the on_removed callback.
*
* \param[in] update_from The collection representing the next iteration's state
* \param[inout] update_to The collection representing the current iteration's state
* \param[in] on_added Callback fired when a new entity is detected
* \param[in] on_removed Callback fired when an entity is removed
*/
template<typename CollectionType>
void update_entities(
const CollectionType & update_from,
CollectionType & update_to,
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_added,
std::function<void(const typename CollectionType::EntitySharedPtr &)> on_removed
)
{
for (auto it = update_to.begin(); it != update_to.end(); ) {
if (update_from.count(it->first) == 0) {
auto entity = it->second.entity.lock();
if (entity) {
on_removed(entity);
}
it = update_to.erase(it);
} else {
++it;
}
}
for (auto it = update_from.begin(); it != update_from.end(); ++it) {
if (update_to.count(it->first) == 0) {
auto entity = it->second.entity.lock();
if (entity) {
on_added(entity);
}
update_to.insert(*it);
}
}
}
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(const EntitySharedPtr &)> on_added,
std::function<void(const EntitySharedPtr &)> on_removed)
{
update_entities(other, *this, on_added, on_removed);
}
};
struct ExecutorEntitiesCollection
{
using TimerCollection = EntityCollection<rcl_timer_t, rclcpp::TimerBase>;
using SubscriptionCollection = EntityCollection<rcl_subscription_t, rclcpp::SubscriptionBase>;
using ClientCollection = EntityCollection<rcl_client_t, rclcpp::ClientBase>;
using ServiceCollection = EntityCollection<rcl_service_t, rclcpp::ServiceBase>;
using WaitableCollection = EntityCollection<rclcpp::Waitable, rclcpp::Waitable>;
using GuardConditionCollection = EntityCollection<rcl_guard_condition_t, rclcpp::GuardCondition>;
TimerCollection timers;
SubscriptionCollection subscriptions;
ClientCollection clients;
ServiceCollection services;
GuardConditionCollection guard_conditions;
WaitableCollection waitables;
bool empty() const;
void clear();
size_t remove_expired_entities();
};
void
build_entities_collection(
const std::vector<rclcpp::CallbackGroup::WeakPtr> & callback_groups,
ExecutorEntitiesCollection & collection);
size_t
ready_executables(
const ExecutorEntitiesCollection & collection,
rclcpp::WaitResult<rclcpp::WaitSet> & wait_result,
std::deque<rclcpp::AnyExecutable> & executables
);
} // namespace executors
} // namespace rclcpp
#endif // RCLCPP__EXECUTORS__EXECUTOR_ENTITIES_COLLECTION_HPP_