service_runner.hpp

Go to the documentation of this file.

#pragma once
 
#include <chrono>
 
#include <capabilities2_runner/threadtrigger_runner.hpp>
 
namespace capabilities2_runner
{
 
template <typename ServiceT>
class ServiceRunner : public ThreadTriggerRunner
{
public:
  ServiceRunner() : ThreadTriggerRunner()
  {
  }
 
  virtual void init_service(rclcpp::Node::SharedPtr node, const runner_opts& run_config,
                            const std::string& service_name)
  {
    // initialize the runner base by storing node pointer and run config
    init_base(node, run_config);
 
    // create a service client
    service_client_ = node_->create_client<ServiceT>(service_name);
 
    // wait for service server
    RCLCPP_INFO(node_->get_logger(), "waiting for service: %s", service_name.c_str());
 
    if (!service_client_->wait_for_service(std::chrono::seconds(3)))
    {
      RCLCPP_ERROR(node_->get_logger(), "failed to connect to service: %s", service_name.c_str());
      throw runner_exception("failed to connect to server");
    }
 
    RCLCPP_INFO(node_->get_logger(), "connected with service: %s", service_name.c_str());
  }
 
  virtual void stop(const std::string& bond_id, const std::string& instance_id = "") override
  {
    // if the node pointer is empty then throw an error
    // this means that the runner was not started and is being used out of order
 
    if (!node_)
      throw runner_exception("cannot stop runner that was not started");
 
    // throw an error if the service client is null
    // this can happen if the runner is not able to find the action resource
 
    if (!service_client_)
      throw runner_exception("cannot stop runner action that was not started");
 
    // emit stopped event
    emit_stopped(bond_id, instance_id, param_on_stopped());
 
    RCLCPP_INFO(node_->get_logger(), "runner cleaned. stopping..");
  }
 
protected:
  virtual void execution(capabilities2_events::EventParameters parameters, const std::string& thread_id) override
  {
    // split thread_id to get bond_id and instance_id (format: "bond_id/instance_id")
    std::string bond_id = ThreadTriggerRunner::bond_from_thread_id(thread_id);
    std::string instance_id = ThreadTriggerRunner::instance_from_thread_id(thread_id);
 
    // generate a goal from parameters if provided
    auto request_msg = std::make_shared<typename ServiceT::Request>(generate_request(parameters));
 
    RCLCPP_INFO(node_->get_logger(), "request generated for event :%s", instance_id.c_str());
 
    std::mutex block_mutex;
    std::unique_lock<std::mutex> lock(block_mutex);
    std::condition_variable cv;
    bool completed = false;
 
    auto result_future = service_client_->async_send_request(
        request_msg, [this,  &instance_id, &completed, &bond_id, &cv](typename rclcpp::Client<ServiceT>::SharedFuture future) {
          if (!future.valid())
          {
            RCLCPP_ERROR(node_->get_logger(), "get result call failed");
 
            // emit failed event
            emit_failed(bond_id, instance_id, param_on_failure());
          }
          else
          {
            RCLCPP_INFO(node_->get_logger(), "get result call succeeded for event :%s", instance_id.c_str());
 
            response_ = future.get();
            process_response(response_);
 
            // emit success event
            emit_succeeded(bond_id, instance_id, param_on_success());
          }
 
          completed = true;
          cv.notify_all();
        });
 
    // Conditional wait
    cv.wait(lock, [&completed] { return completed; });
    RCLCPP_INFO(node_->get_logger(), "Service request complete. Thread closing.");
  }
 
protected:
  virtual typename ServiceT::Request generate_request(capabilities2_events::EventParameters& parameters) = 0;
 
  virtual void process_response(typename ServiceT::Response::SharedPtr /*response*/) {}
 
  typename rclcpp::Client<ServiceT>::SharedPtr service_client_;
  typename ServiceT::Response::SharedPtr response_;
};
}  // namespace capabilities2_runner