Program Listing for File events_executor.hpp

↰ Return to documentation for file (include/rclcpp/experimental/executors/events_executor/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_