events_executor.hpp

// Copyright 2023 iRobot Corporation.
//
// 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__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
#define RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_
 
#include <atomic>
#include <chrono>
#include <memory>
#include <vector>
 
#include "rclcpp/executor.hpp"
#include "rclcpp/executors/executor_entities_collection.hpp"
#include "rclcpp/executors/executor_entities_collector.hpp"
#include "rclcpp/experimental/executors/events_executor/events_executor_event_types.hpp"
#include "rclcpp/experimental/executors/events_executor/events_queue.hpp"
#include "rclcpp/experimental/executors/events_executor/simple_events_queue.hpp"
#include "rclcpp/experimental/timers_manager.hpp"
#include "rclcpp/node.hpp"
 
namespace rclcpp
{
namespace experimental
{
namespace executors
{
 
 
class EventsExecutor : public rclcpp::Executor
{
  friend class EventsExecutorEntitiesCollector;
 
public:
  RCLCPP_SMART_PTR_DEFINITIONS(EventsExecutor)
 
  
  RCLCPP_PUBLIC
  explicit EventsExecutor(
    rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue = std::make_unique<
      rclcpp::experimental::executors::SimpleEventsQueue>(),
    bool execute_timers_separate_thread = false,
    const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
 
  RCLCPP_PUBLIC
  virtual ~EventsExecutor();
 
 
  RCLCPP_PUBLIC
  void
  spin() override;
 
 
  RCLCPP_PUBLIC
  void
  spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override;
 
 
  RCLCPP_PUBLIC
  void
  spin_all(std::chrono::nanoseconds max_duration) override;
 
 
  RCLCPP_PUBLIC
  void
  add_node(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
    bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  void
  add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  void
  remove_node(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
    bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  void
  remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  void
  add_callback_group(
    rclcpp::CallbackGroup::SharedPtr group_ptr,
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
    bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  void
  remove_callback_group(
    rclcpp::CallbackGroup::SharedPtr group_ptr,
    bool notify = true) override;
 
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_all_callback_groups() override;
 
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_manually_added_callback_groups() override;
 
 
  RCLCPP_PUBLIC
  std::vector<rclcpp::CallbackGroup::WeakPtr>
  get_automatically_added_callback_groups_from_nodes() override;
 
protected:
  RCLCPP_PUBLIC
  void
  spin_once_impl(std::chrono::nanoseconds timeout) override;
 
  RCLCPP_PUBLIC
  void
  spin_some_impl(std::chrono::nanoseconds max_duration, bool exhaustive);
 
private:
  RCLCPP_DISABLE_COPY(EventsExecutor)
 
  
  void
  execute_event(const ExecutorEvent & event);
 
  void
  refresh_current_collection_from_callback_groups();
 
  void
  refresh_current_collection(const rclcpp::executors::ExecutorEntitiesCollection & new_collection);
 
  std::function<void(size_t)>
  create_entity_callback(void * entity_key, ExecutorEventType type);
 
  std::function<void(size_t, int)>
  create_waitable_callback(const rclcpp::Waitable * waitable_id);
 
  void
  add_notify_waitable_to_collection(
    rclcpp::executors::ExecutorEntitiesCollection::WaitableCollection & collection);
 
  template<typename CollectionType>
  typename CollectionType::EntitySharedPtr
  retrieve_entity(typename CollectionType::Key entity_id, CollectionType & collection)
  {
    // Check if the entity_id is in the collection
    auto it = collection.find(entity_id);
    if (it == collection.end()) {
      return nullptr;
    }
 
    // Check if the entity associated with the entity_id is valid
    // and remove it from the collection if it isn't
    auto entity = it->second.entity.lock();
    if (!entity) {
      collection.erase(it);
    }
 
    // Return the retrieved entity (this can be a nullptr if the entity was not valid)
    return entity;
  }
 
  rclcpp::experimental::executors::EventsQueue::UniquePtr events_queue_;
 
  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollector> entities_collector_;
  std::shared_ptr<rclcpp::executors::ExecutorNotifyWaitable> notify_waitable_;
 
  std::recursive_mutex collection_mutex_;
  std::shared_ptr<rclcpp::executors::ExecutorEntitiesCollection> current_entities_collection_;
 
  std::atomic<bool> notify_waitable_event_pushed_ {false};
 
  std::shared_ptr<rclcpp::experimental::TimersManager> timers_manager_;
};
 
}  // namespace executors
}  // namespace experimental
}  // namespace rclcpp
 
#endif  // RCLCPP__EXPERIMENTAL__EXECUTORS__EVENTS_EXECUTOR__EVENTS_EXECUTOR_HPP_