text: // Copyright 2019 Nobleo Technology
  //
  // 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__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
  #define RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
  
  #include <cassert>
  #include <cstdlib>
  #include <memory>
  #include <vector>
  #include <string>
  
  #include "rmw/rmw.h"
  
  #include "rclcpp/executor.hpp"
  #include "rclcpp/executors/static_executor_entities_collector.hpp"
  #include "rclcpp/experimental/executable_list.hpp"
  #include "rclcpp/macros.hpp"
  #include "rclcpp/memory_strategies.hpp"
  #include "rclcpp/node.hpp"
  #include "rclcpp/rate.hpp"
  #include "rclcpp/utilities.hpp"
  #include "rclcpp/visibility_control.hpp"
  
  namespace rclcpp
  {
  namespace executors
  {
  
  /// Static executor implementation
  /**
   * This executor is a static version of the original single threaded executor.
   * It's static because it doesn't reconstruct the executable list for every iteration.
   * All nodes, callbackgroups, timers, subscriptions etc. are created before
   * spin() is called, and modified only when an entity is added/removed to/from a node.
   *
   * To run this executor instead of SingleThreadedExecutor replace:
   * rclcpp::executors::SingleThreadedExecutor exec;
   * by
   * rclcpp::executors::StaticSingleThreadedExecutor exec;
   * in your source code and spin node(s) in the following way:
   * exec.add_node(node);
   * exec.spin();
   * exec.remove_node(node);
   */
  class StaticSingleThreadedExecutor : public rclcpp::Executor
  {
  public:
    RCLCPP_SMART_PTR_DEFINITIONS(StaticSingleThreadedExecutor)
  
    /// Default constructor. See the default constructor for Executor.
    RCLCPP_PUBLIC
    explicit StaticSingleThreadedExecutor(
      const rclcpp::ExecutorOptions & options = rclcpp::ExecutorOptions());
  
    /// Default destrcutor.
    RCLCPP_PUBLIC
    virtual ~StaticSingleThreadedExecutor();
  
    /// Static executor implementation of spin.
    /**
     * This function will block until work comes in, execute it, and keep blocking.
     * It will only be interrupted by a CTRL-C (managed by the global signal handler).
     * \throws std::runtime_error when spin() called while already spinning
     */
    RCLCPP_PUBLIC
    void
    spin() override;
  
    /// Add a callback group to an executor.
    /**
     * \sa rclcpp::Executor::add_callback_group
     */
    RCLCPP_PUBLIC
    void
    add_callback_group(
      rclcpp::CallbackGroup::SharedPtr group_ptr,
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
      bool notify = true) override;
  
    /// Remove callback group from the executor
    /**
     * \sa rclcpp::Executor::remove_callback_group
     */
    RCLCPP_PUBLIC
    void
    remove_callback_group(
      rclcpp::CallbackGroup::SharedPtr group_ptr,
      bool notify = true) override;
  
    /// Add a node to the executor.
    /**
     * \sa rclcpp::Executor::add_node
     */
    RCLCPP_PUBLIC
    void
    add_node(
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
      bool notify = true) override;
  
    /// Convenience function which takes Node and forwards NodeBaseInterface.
    /**
     * \sa rclcpp::StaticSingleThreadedExecutor::add_node
     */
    RCLCPP_PUBLIC
    void
    add_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
  
    /// Remove a node from the executor.
    /**
     * \sa rclcpp::Executor::remove_node
     */
    RCLCPP_PUBLIC
    void
    remove_node(
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_ptr,
      bool notify = true) override;
  
    /// Convenience function which takes Node and forwards NodeBaseInterface.
    /**
     * \sa rclcpp::Executor::remove_node
     */
    RCLCPP_PUBLIC
    void
    remove_node(std::shared_ptr<rclcpp::Node> node_ptr, bool notify = true) override;
  
    RCLCPP_PUBLIC
    std::vector<rclcpp::CallbackGroup::WeakPtr>
    get_all_callback_groups() override;
  
    /// Get callback groups that belong to executor.
    /**
     * \sa rclcpp::Executor::get_manually_added_callback_groups()
     */
    RCLCPP_PUBLIC
    std::vector<rclcpp::CallbackGroup::WeakPtr>
    get_manually_added_callback_groups() override;
  
    /// Get callback groups that belong to executor.
    /**
     * \sa rclcpp::Executor::get_automatically_added_callback_groups_from_nodes()
     */
    RCLCPP_PUBLIC
    std::vector<rclcpp::CallbackGroup::WeakPtr>
    get_automatically_added_callback_groups_from_nodes() override;
  
    /// Spin (blocking) until the future is complete, it times out waiting, or rclcpp is interrupted.
    /**
     * \param[in] future The future to wait on. If this function returns SUCCESS, the future can be
     *   accessed without blocking (though it may still throw an exception).
     * \param[in] timeout Optional timeout parameter, which gets passed to
     *    Executor::execute_ready_executables.
     *   `-1` is block forever, `0` is non-blocking.
     *   If the time spent inside the blocking loop exceeds this timeout, return a TIMEOUT return
     *   code.
     * \return The return code, one of `SUCCESS`, `INTERRUPTED`, or `TIMEOUT`.
     *
     *  Example usage:
     *  rclcpp::executors::StaticSingleThreadedExecutor exec;
     *  // ... other part of code like creating node
     *  // define future
     *  exec.add_node(node);
     *  exec.spin_until_future_complete(future);
     */
    template<typename FutureT, typename TimeRepT = int64_t, typename TimeT = std::milli>
    rclcpp::FutureReturnCode
    spin_until_future_complete(
      FutureT & future,
      std::chrono::duration<TimeRepT, TimeT> timeout = std::chrono::duration<TimeRepT, TimeT>(-1))
    {
      std::future_status status = future.wait_for(std::chrono::seconds(0));
      if (status == std::future_status::ready) {
        return rclcpp::FutureReturnCode::SUCCESS;
      }
  
      auto end_time = std::chrono::steady_clock::now();
      std::chrono::nanoseconds timeout_ns = std::chrono::duration_cast<std::chrono::nanoseconds>(
        timeout);
      if (timeout_ns > std::chrono::nanoseconds::zero()) {
        end_time += timeout_ns;
      }
      std::chrono::nanoseconds timeout_left = timeout_ns;
  
      entities_collector_->init(&wait_set_, memory_strategy_, &interrupt_guard_condition_);
  
      while (rclcpp::ok(this->context_)) {
        // Do one set of work.
        entities_collector_->refresh_wait_set(timeout_left);
        execute_ready_executables();
        // Check if the future is set, return SUCCESS if it is.
        status = future.wait_for(std::chrono::seconds(0));
        if (status == std::future_status::ready) {
          return rclcpp::FutureReturnCode::SUCCESS;
        }
        // If the original timeout is < 0, then this is blocking, never TIMEOUT.
        if (timeout_ns < std::chrono::nanoseconds::zero()) {
          continue;
        }
        // Otherwise check if we still have time to wait, return TIMEOUT if not.
        auto now = std::chrono::steady_clock::now();
        if (now >= end_time) {
          return rclcpp::FutureReturnCode::TIMEOUT;
        }
        // Subtract the elapsed time from the original timeout.
        timeout_left = std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - now);
      }
  
      // The future did not complete before ok() returned false, return INTERRUPTED.
      return rclcpp::FutureReturnCode::INTERRUPTED;
    }
  
    /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
    RCLCPP_PUBLIC
    void
    spin_some(std::chrono::nanoseconds max_duration = std::chrono::nanoseconds(0)) override
    {
      (void)max_duration;
      throw rclcpp::exceptions::UnimplementedError(
              "spin_some is not implemented for StaticSingleThreadedExecutor, use spin or "
              "spin_until_future_complete");
    }
  
    /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
    RCLCPP_PUBLIC
    void
    spin_all(std::chrono::nanoseconds) override
    {
      throw rclcpp::exceptions::UnimplementedError(
              "spin_all is not implemented for StaticSingleThreadedExecutor, use spin or "
              "spin_until_future_complete");
    }
  
    /// Not yet implemented, see https://github.com/ros2/rclcpp/issues/1219 for tracking
    RCLCPP_PUBLIC
    void
    spin_once(std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1)) override
    {
      (void)timeout;
      throw rclcpp::exceptions::UnimplementedError(
              "spin_once is not implemented for StaticSingleThreadedExecutor, use spin or "
              "spin_until_future_complete");
    }
  
  protected:
    /// Check which executables in ExecutableList struct are ready from wait_set and execute them.
    /**
     * \param[in] exec_list Structure that can hold subscriptionbases, timerbases, etc
     * \param[in] timeout Optional timeout parameter.
     */
    RCLCPP_PUBLIC
    void
    execute_ready_executables();
  
  private:
    RCLCPP_DISABLE_COPY(StaticSingleThreadedExecutor)
  
    StaticExecutorEntitiesCollector::SharedPtr entities_collector_;
  };
  
  }  // namespace executors
  }  // namespace rclcpp
  
  #endif  // RCLCPP__EXECUTORS__STATIC_SINGLE_THREADED_EXECUTOR_HPP_
  