service_state.hpp

Go to the documentation of this file.

// Copyright (C) 2023 Miguel Ángel González Santamarta
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
#ifndef YASMIN_ROS__SERVICE_STATE_HPP
#define YASMIN_ROS__SERVICE_STATE_HPP
 
#include <condition_variable>
#include <functional>
#include <memory>
#include <mutex>
#include <set>
#include <string>
 
#include "rclcpp/rclcpp.hpp"
 
#include "yasmin/blackboard.hpp"
#include "yasmin/logs.hpp"
#include "yasmin/state.hpp"
#include "yasmin_ros/basic_outcomes.hpp"
#include "yasmin_ros/ros_clients_cache.hpp"
#include "yasmin_ros/yasmin_node.hpp"
 
using namespace std::placeholders;
 
namespace yasmin_ros {

template <typename ServiceT> class ServiceState : public yasmin::State {
  using Request = typename ServiceT::Request::SharedPtr;
  using Response = typename ServiceT::Response::SharedPtr;

  using CreateRequestHandler =
      std::function<Request(std::shared_ptr<yasmin::Blackboard>)>;
  using ResponseHandler =
      std::function<std::string(std::shared_ptr<yasmin::Blackboard>, Response)>;
 
public:
  ServiceState(const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               int wait_timeout = -1, int response_timeout = -1,
               int maximum_retry = 3)
      : ServiceState(nullptr, srv_name, create_request_handler,
                     std::set<std::string>(), nullptr, nullptr, wait_timeout,
                     response_timeout, maximum_retry) {}

  ServiceState(const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               const std::set<std::string> &outcomes, int wait_timeout = -1,
               int response_timeout = -1, int maximum_retry = 3)
      : ServiceState(nullptr, srv_name, create_request_handler, outcomes,
                     nullptr, nullptr, wait_timeout, response_timeout,
                     maximum_retry) {}

  ServiceState(const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               const std::set<std::string> &outcomes,
               rclcpp::CallbackGroup::SharedPtr callback_group = nullptr,
               int wait_timeout = -1, int response_timeout = -1,
               int maximum_retry = 3)
      : ServiceState(nullptr, srv_name, create_request_handler, outcomes,
                     nullptr, callback_group, wait_timeout, response_timeout,
                     maximum_retry) {}

  ServiceState(const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               ResponseHandler response_handler, int wait_timeout = -1,
               int response_timeout = -1, int maximum_retry = 3)
      : ServiceState(nullptr, srv_name, create_request_handler, {},
                     response_handler, nullptr, wait_timeout, response_timeout,
                     maximum_retry) {}

  ServiceState(const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               const std::set<std::string> &outcomes,
               ResponseHandler response_handler, int wait_timeout = -1,
               int response_timeout = -1, int maximum_retry = 3)
      : ServiceState(nullptr, srv_name, create_request_handler, outcomes,
                     response_handler, nullptr, wait_timeout, response_timeout,
                     maximum_retry) {}

  ServiceState(const rclcpp::Node::SharedPtr &node, const std::string &srv_name,
               CreateRequestHandler create_request_handler,
               const std::set<std::string> &outcomes,
               ResponseHandler response_handler,
               rclcpp::CallbackGroup::SharedPtr callback_group,
               int wait_timeout = -1, int response_timeout = -1,
               int maximum_retry = 3)
      : State({basic_outcomes::SUCCEED, basic_outcomes::ABORT}),
        srv_name(srv_name), wait_timeout(wait_timeout),
        response_timeout(response_timeout), maximum_retry(maximum_retry) {
 
    if (this->wait_timeout > 0 || this->response_timeout > 0) {
      this->outcomes.insert(basic_outcomes::TIMEOUT);
    }
 
    if (!outcomes.empty()) {
      this->outcomes.insert(outcomes.begin(), outcomes.end());
    }
 
    // Assign the appropriate ROS 2 node
    if (node == nullptr) {
      this->node_ = YasminNode::get_instance();
    } else {
      this->node_ = node;
    }
 
    // Create a service client (compatible with different rclcpp versions)
    this->service_client =
        ROSClientsCache::get_or_create_service_client<ServiceT>(
            this->node_, srv_name, callback_group);
 
    // Set the request and response handlers
    this->create_request_handler = create_request_handler;
    this->response_handler = response_handler;
 
    // Validate request handler
    if (this->create_request_handler == nullptr) {
      throw std::invalid_argument("create_request_handler is needed");
    }
  }

  std::string execute(std::shared_ptr<yasmin::Blackboard> blackboard) override {
    Request request = this->create_request(blackboard);
    std::unique_lock<std::mutex> lock(this->response_done_mutex);
    int retry_count = 0;
 
    // Wait for the service to become available
    YASMIN_LOG_INFO("Waiting for service '%s'", this->srv_name.c_str());
 
    while (!this->service_client->wait_for_service(
        std::chrono::duration<int64_t, std::ratio<1>>(this->wait_timeout))) {
      YASMIN_LOG_WARN("Timeout reached, service '%s' is not available",
                      this->srv_name.c_str());
      if (retry_count < this->maximum_retry) {
        retry_count++;
        YASMIN_LOG_WARN("Retrying to connect to service '%s' "
                        "(%d/%d)",
                        this->srv_name.c_str(), retry_count,
                        this->maximum_retry);
      } else {
        return basic_outcomes::TIMEOUT;
      }
    }
 
    // Send the service request
    YASMIN_LOG_INFO("Sending request to service '%s'", this->srv_name.c_str());
 
    // Send request with callback
    this->service_client->async_send_request(
        request, std::bind(&ServiceState::response_callback, this, _1));
 
    // Wait for response with timeout
    if (this->response_timeout > 0) {
      while (this->response_done_cond.wait_for(
                 lock, std::chrono::seconds(this->response_timeout)) ==
             std::cv_status::timeout) {
        YASMIN_LOG_WARN(
            "Timeout reached while waiting for response from service '%s'",
            this->srv_name.c_str());
        if (retry_count < this->maximum_retry) {
          retry_count++;
          YASMIN_LOG_WARN("Retrying to wait for service '%s' response (%d/%d)",
                          this->srv_name.c_str(), retry_count,
                          this->maximum_retry);
        } else {
          return basic_outcomes::TIMEOUT;
        }
      }
    } else {
      this->response_done_cond.wait(lock);
    }
 
    if (this->is_canceled()) {
      return basic_outcomes::CANCEL;
    }
 
    // Process the service response
    if (this->service_response) {
      if (response_handler != nullptr) {
        std::string outcome =
            this->response_handler(blackboard, this->service_response);
        return outcome;
      }
      return basic_outcomes::SUCCEED;
    } else {
      return basic_outcomes::ABORT;
    }
  }
 
protected:
  rclcpp::Node::SharedPtr node_;
 
private:
  std::shared_ptr<rclcpp::Client<ServiceT>> service_client;
  CreateRequestHandler create_request_handler;
  ResponseHandler response_handler;
  std::string srv_name;
  int wait_timeout;
  int response_timeout;
  int maximum_retry;

  std::condition_variable response_done_cond;
  std::mutex response_done_mutex;
  Response service_response;

  Request create_request(std::shared_ptr<yasmin::Blackboard> blackboard) {
    return this->create_request_handler(blackboard);
  }

  void
  response_callback(typename rclcpp::Client<ServiceT>::SharedFuture response) {
    std::lock_guard<std::mutex> lock(this->response_done_mutex);
    this->service_response = response.get();
    this->response_done_cond.notify_one();
  }
};
 
} // namespace yasmin_ros
 
#endif // YASMIN_ROS__SERVICE_STATE_HPP