async_function_handler.hpp

// Copyright 2024 PAL Robotics S.L.
//
// 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 REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_
#define REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_
 
#include <atomic>
#include <condition_variable>
#include <functional>
#include <limits>
#include <memory>
#include <mutex>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>
 
#include "rclcpp/duration.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/time.hpp"
#include "realtime_tools/realtime_helpers.hpp"
 
namespace realtime_tools
{
template <typename T>
class AsyncFunctionHandler
{
public:
  AsyncFunctionHandler() = default;
 
  ~AsyncFunctionHandler() { stop_thread(); }
 
 
  void init(
    std::function<T(const rclcpp::Time &, const rclcpp::Duration &)> callback,
    int thread_priority = 50)
  {
    if (callback == nullptr) {
      throw std::runtime_error(
        "AsyncFunctionHandler: parsed function to call asynchronously is not valid!");
    }
    if (thread_.joinable()) {
      throw std::runtime_error(
        "AsyncFunctionHandler: Cannot reinitialize while the thread is "
        "running. Please stop the async callback first!");
    }
    async_function_ = callback;
    thread_priority_ = thread_priority;
  }
 
 
  void init(
    std::function<T(const rclcpp::Time &, const rclcpp::Duration &)> callback,
    std::function<bool()> trigger_predicate, int thread_priority = 50)
  {
    if (trigger_predicate == nullptr) {
      throw std::runtime_error("AsyncFunctionHandler: parsed trigger predicate is not valid!");
    }
    init(callback, thread_priority);
    trigger_predicate_ = trigger_predicate;
  }
 
 
  std::pair<bool, T> trigger_async_callback(
    const rclcpp::Time & time, const rclcpp::Duration & period)
  {
    if (!is_initialized()) {
      throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
    }
    if (async_exception_ptr_) {
      RCLCPP_ERROR(
        rclcpp::get_logger("AsyncFunctionHandler"),
        "AsyncFunctionHandler: Exception caught in the async callback thread!");
      std::rethrow_exception(async_exception_ptr_);
    }
    if (!is_running()) {
      throw std::runtime_error(
        "AsyncFunctionHandler: need to start the async callback thread first before triggering!");
    }
    std::unique_lock<std::mutex> lock(async_mtx_, std::try_to_lock);
    bool trigger_status = false;
    if (lock.owns_lock() && !trigger_in_progress_ && trigger_predicate_()) {
      {
        std::unique_lock<std::mutex> scoped_lock(std::move(lock));
        trigger_in_progress_ = true;
        current_callback_time_ = time;
        current_callback_period_ = period;
      }
      async_callback_condition_.notify_one();
      trigger_status = true;
    }
    const T return_value = async_callback_return_;
    return std::make_pair(trigger_status, return_value);
  }
 
 
  T get_last_return_value() const { return async_callback_return_; }
 
 
  const rclcpp::Time & get_current_callback_time() const { return current_callback_time_; }
 
 
  const rclcpp::Duration & get_current_callback_period() const { return current_callback_period_; }
 
 
  void reset_variables()
  {
    std::unique_lock<std::mutex> lock(async_mtx_);
    stop_async_callback_ = false;
    trigger_in_progress_ = false;
    current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
    current_callback_period_ = rclcpp::Duration(0, 0);
    last_execution_time_ = std::chrono::nanoseconds(0);
    async_callback_return_ = T();
    async_exception_ptr_ = nullptr;
  }
 
 
  void wait_for_trigger_cycle_to_finish()
  {
    if (is_running()) {
      std::unique_lock<std::mutex> lock(async_mtx_);
      cycle_end_condition_.wait(lock, [this] { return !trigger_in_progress_; });
    }
  }
 
 
  bool is_initialized() const { return async_function_ && trigger_predicate_; }
 
 
  void join_async_callback_thread()
  {
    if (is_running()) {
      thread_.join();
    }
  }
 
 
  bool is_running() const { return thread_.joinable(); }
 
 
  bool is_stopped() const { return stop_async_callback_; }
 
 
  std::thread & get_thread() { return thread_; }
 
 
  const std::thread & get_thread() const { return thread_; }
 
 
  bool is_trigger_cycle_in_progress() const { return trigger_in_progress_; }
 
 
  void stop_thread()
  {
    if (is_running()) {
      {
        std::unique_lock<std::mutex> lock(async_mtx_);
        stop_async_callback_ = true;
      }
      async_callback_condition_.notify_one();
      thread_.join();
    }
  }
 
 
  std::chrono::nanoseconds get_last_execution_time() const
  {
    return last_execution_time_.load(std::memory_order_relaxed);
  }
 
 
  void start_thread()
  {
    if (!is_initialized()) {
      throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!");
    }
    if (!thread_.joinable()) {
      reset_variables();
      thread_ = std::thread([this]() -> void {
        if (!realtime_tools::configure_sched_fifo(thread_priority_)) {
          RCLCPP_WARN(
            rclcpp::get_logger("AsyncFunctionHandler"),
            "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO "
            "RT "
            "scheduling. See "
            "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] "
            "for details.");
        }
 
        while (!stop_async_callback_.load(std::memory_order_relaxed)) {
          {
            std::unique_lock<std::mutex> lock(async_mtx_);
            async_callback_condition_.wait(
              lock, [this] { return trigger_in_progress_ || stop_async_callback_; });
            if (!stop_async_callback_) {
              const auto start_time = std::chrono::steady_clock::now();
              try {
                async_callback_return_ =
                  async_function_(current_callback_time_, current_callback_period_);
              } catch (...) {
                async_exception_ptr_ = std::current_exception();
              }
              const auto end_time = std::chrono::steady_clock::now();
              last_execution_time_ =
                std::chrono::duration_cast<std::chrono::nanoseconds>(end_time - start_time);
            }
            trigger_in_progress_ = false;
          }
          cycle_end_condition_.notify_all();
        }
      });
    }
  }
 
private:
  rclcpp::Time current_callback_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED);
  rclcpp::Duration current_callback_period_{0, 0};
 
  std::function<T(const rclcpp::Time &, const rclcpp::Duration &)> async_function_;
  std::function<bool()> trigger_predicate_ = []() { return true; };
 
  // Async related variables
  std::thread thread_;
  int thread_priority_ = std::numeric_limits<int>::quiet_NaN();
  std::atomic_bool stop_async_callback_{false};
  std::atomic_bool trigger_in_progress_{false};
  std::atomic<T> async_callback_return_;
  std::condition_variable async_callback_condition_;
  std::condition_variable cycle_end_condition_;
  std::mutex async_mtx_;
  std::atomic<std::chrono::nanoseconds> last_execution_time_;
  std::exception_ptr async_exception_ptr_;
};
}  // namespace realtime_tools
 
#endif  // REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_