threadtrigger_runner.hpp

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <map>
#include <atomic>
#include <mutex>
#include <thread>
 
#include <capabilities2_runner/runner_base.hpp>
#include <capabilities2_events/uuid_generator.hpp>
#include <capabilities2_msgs/msg/capability_event_code.hpp>
 
namespace capabilities2_runner
{
 
class ThreadTriggerRunner : public RunnerBase
{
public:
  static const std::string bond_from_thread_id(const std::string& thread_id)
  {
    // extract bond_id from thread_id (format: "bond_id/trigger_id")
    size_t slash_pos = thread_id.find('/');
    std::string bond_id = (slash_pos != std::string::npos) ? thread_id.substr(0, slash_pos) : thread_id;
    return bond_id;
  }
 
  static const std::string instance_from_thread_id(const std::string& thread_id)
  {
    // extract instance_id from thread_id (format: "bond_id/instance_id")
    size_t slash_pos = thread_id.find('/');
    std::string instance_id = (slash_pos != std::string::npos) ? thread_id.substr(slash_pos + 1) : "";
    return instance_id;
  }
 
public:
  ThreadTriggerRunner() : RunnerBase(), execution_thread_pool_(), mutex_(), execution_should_stop_(false)
  {
  }
 
  ~ThreadTriggerRunner()
  {
    // stop any running threads on destruction
    stop_execution(std::chrono::milliseconds(500));
  }
 
  virtual void trigger(capabilities2_events::EventParameters& parameters, const std::string& bond_id,
                       const std::string& instance_id) override
  {
    // namespace the thread id with bond id for later
    // could list all threads related to a bond if needed
    std::string thread_id = bond_id + "/" + instance_id;
 
    // start execution thread
    {
      std::scoped_lock lock(mutex_);
      // TODO: consider emitting on start event here
      // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STARTED, updated_on_started(parameters));
      execution_thread_pool_[thread_id] = std::thread(&ThreadTriggerRunner::execution, this, parameters, thread_id);
    }
 
    // emit trigger event
    emit_event(bond_id, instance_id, capabilities2_msgs::msg::CapabilityEventCode::TRIGGERED);
 
    // BUG: thread management?
 
    // TODO: consider emitting on stop event here
    // emit_event(bond_id, capabilities2_msgs::msg::CapabilityEventCode::ON_STOPPED, updated_on_stopped(parameters));
 
    RCLCPP_DEBUG(node_->get_logger(), "started execution thread for thread id: %s", thread_id.c_str());
  }
 
protected:
  virtual void execution(capabilities2_events::EventParameters parameters, const std::string& thread_id) = 0;
 
private:
  void stop_execution(std::chrono::milliseconds timeout = std::chrono::milliseconds(500))
  {
    // signal listening threads to stop
    execution_should_stop_ = true;
 
    // try join in a non-blocking way and log if threads are not cleaned up properly
    {
      std::scoped_lock lock(mutex_);
      for (auto& [thread_id, exec_thread] : execution_thread_pool_)
      {
        if (exec_thread.joinable())
        {
          // TODO: FIX THIS FUTURE MICHAEL
          exec_thread.join();
        }
      }
 
      // clear the thread pool
      execution_thread_pool_.clear();
    }
  }
 
protected:
  std::map<std::string, std::thread> execution_thread_pool_;
 
  std::mutex mutex_;
 
  std::atomic<bool> execution_should_stop_;
};
 
}  // namespace capabilities2_runner