Program Listing for File service_state.hpp

↰ Return to documentation for file (include/yasmin_ros/service_state.hpp)

// 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 <functional>
#include <memory>
#include <set>
#include <string>

#include "rclcpp/rclcpp.hpp"

#include "yasmin/blackboard/blackboard.hpp"
#include "yasmin/state.hpp"
#include "yasmin_ros/basic_outcomes.hpp"
#include "yasmin_ros/yasmin_node.hpp"

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::Blackboard>)>;
  using ResponseHandler = std::function<std::string(
      std::shared_ptr<yasmin::blackboard::Blackboard>, Response)>;

public:
  ServiceState(std::string srv_name,
               CreateRequestHandler create_request_handler,
               std::set<std::string> outcomes, int timeout = -1.0)
      : ServiceState(srv_name, create_request_handler, outcomes, nullptr,
                     timeout) {}

  ServiceState(std::string srv_name,
               CreateRequestHandler create_request_handler,
               std::set<std::string> outcomes, ResponseHandler response_handler,
               int timeout = -1.0)
      : ServiceState(nullptr, srv_name, create_request_handler, outcomes,
                     response_handler, timeout) {}

  ServiceState(const rclcpp::Node::SharedPtr &node, std::string srv_name,
               CreateRequestHandler create_request_handler,
               std::set<std::string> outcomes, ResponseHandler response_handler,
               int timeout = -1.0)
      : State({}), srv_name(srv_name), timeout(timeout) {

    this->outcomes = {basic_outcomes::SUCCEED, basic_outcomes::ABORT};

    if (this->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
    this->service_client =
        this->node_->template create_client<ServiceT>(srv_name);

    // 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> blackboard) override {

    Request request = this->create_request(blackboard);

    // Wait for the service to become available
    RCLCPP_INFO(this->node_->get_logger(), "Waiting for service '%s'",
                this->srv_name.c_str());
    bool srv_available = this->service_client->wait_for_service(
        std::chrono::duration<int64_t, std::ratio<1>>(this->timeout));

    if (!srv_available) {
      RCLCPP_WARN(this->node_->get_logger(),
                  "Timeout reached, service '%s' is not available",
                  this->srv_name.c_str());
      return basic_outcomes::TIMEOUT;
    }

    // Send the service request
    RCLCPP_INFO(this->node_->get_logger(), "Sending request to service '%s'",
                this->srv_name.c_str());
    auto future = this->service_client->async_send_request(request);

    // Wait for the response
    future.wait();
    Response response = future.get();

    if (response) {
      if (response_handler != nullptr) {
        std::string outcome = this->response_handler(blackboard, response);
        return outcome;
      }
      return basic_outcomes::SUCCEED;
    } else {
      return basic_outcomes::ABORT;
    }
  }

private:
  rclcpp::Node::SharedPtr node_;
  std::shared_ptr<rclcpp::Client<ServiceT>> service_client;
  CreateRequestHandler create_request_handler;
  ResponseHandler response_handler;
  std::string srv_name;
  int timeout;

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

} // namespace yasmin_ros

#endif // YASMIN_ROS__SERVICE_STATE_HPP