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_