action_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__ACTION_STATE_HPP
#define YASMIN_ROS__ACTION_STATE_HPP
 
#include <condition_variable>
#include <functional>
#include <memory>
#include <set>
#include <string>
 
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.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 ActionT> class ActionState : public yasmin::State {
  using Goal = typename ActionT::Goal;
  using Result = typename ActionT::Result::SharedPtr;

  using Feedback = typename ActionT::Feedback;
  using SendGoalOptions =
      typename rclcpp_action::Client<ActionT>::SendGoalOptions;
  using ActionClient = typename rclcpp_action::Client<ActionT>::SharedPtr;
  using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
  using CreateGoalHandler =
      std::function<Goal(std::shared_ptr<yasmin::Blackboard>)>;
  using ResultHandler =
      std::function<std::string(std::shared_ptr<yasmin::Blackboard>, Result)>;
  using FeedbackHandler = std::function<void(
      std::shared_ptr<yasmin::Blackboard>, std::shared_ptr<const Feedback>)>;
 
public:
  ActionState(const std::string &action_name,
              CreateGoalHandler create_goal_handler,
              const std::set<std::string> &outcomes, int wait_timeout = -1,
              int response_timeout = -1, int maximum_retry = 3)
      : ActionState(nullptr, action_name, create_goal_handler, outcomes,
                    nullptr, nullptr, nullptr, wait_timeout, response_timeout,
                    maximum_retry) {}

  ActionState(const std::string &action_name,
              CreateGoalHandler create_goal_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)
      : ActionState(nullptr, action_name, create_goal_handler, outcomes,
                    nullptr, nullptr, callback_group, wait_timeout,
                    response_timeout, maximum_retry) {}

  ActionState(const std::string &action_name,
              CreateGoalHandler create_goal_handler,
              ResultHandler result_handler = nullptr,
              FeedbackHandler feedback_handler = nullptr, int wait_timeout = -1,
              int response_timeout = -1, int maximum_retry = 3)
      : ActionState(nullptr, action_name, create_goal_handler, {},
                    result_handler, feedback_handler, nullptr, wait_timeout,
                    response_timeout, maximum_retry) {}

  ActionState(const std::string &action_name,
              CreateGoalHandler create_goal_handler,
              const std::set<std::string> &outcomes,
              ResultHandler result_handler = nullptr,
              FeedbackHandler feedback_handler = nullptr, int wait_timeout = -1,
              int response_timeout = -1, int maximum_retry = 3)
      : ActionState(nullptr, action_name, create_goal_handler, outcomes,
                    result_handler, feedback_handler, nullptr, wait_timeout,
                    response_timeout, maximum_retry) {}

  ActionState(const rclcpp::Node::SharedPtr &node,
              const std::string &action_name,
              CreateGoalHandler create_goal_handler,
              const std::set<std::string> &outcomes,
              ResultHandler result_handler = nullptr,
              FeedbackHandler feedback_handler = nullptr,
              rclcpp::CallbackGroup::SharedPtr callback_group = nullptr,
              int wait_timeout = -1, int response_timeout = -1,
              int maximum_retry = 3)
      : State({basic_outcomes::SUCCEED, basic_outcomes::ABORT,
               basic_outcomes::CANCEL}),
        action_name(action_name), create_goal_handler(create_goal_handler),
        result_handler(result_handler), feedback_handler(feedback_handler),
        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.size() > 0) {
      for (const std::string &outcome : outcomes) {
        this->outcomes.insert(outcome);
      }
    }
 
    if (node == nullptr) {
      this->node_ = YasminNode::get_instance();
    } else {
      this->node_ = node;
    }
 
    this->action_client = ROSClientsCache::get_or_create_action_client<ActionT>(
        this->node_, action_name, callback_group);
 
    if (this->create_goal_handler == nullptr) {
      throw std::invalid_argument("create_goal_handler is needed");
    }
  }

  void cancel_state() {
    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
 
    if (this->goal_handle) {
      this->action_client->async_cancel_goal(
          this->goal_handle, std::bind(&ActionState::cancel_done, this));
 
      std::unique_lock<std::mutex> lock(this->action_cancel_mutex);
      this->action_cancel_cond.wait(lock);
    }
 
    yasmin::State::cancel_state();
  }

  void cancel_done() { this->action_cancel_cond.notify_all(); }

  std::string execute(std::shared_ptr<yasmin::Blackboard> blackboard) {
 
    std::unique_lock<std::mutex> lock(this->action_done_mutex);
    int retry_count = 0;
 
    Goal goal = this->create_goal_handler(blackboard);
 
    // Wait for the action server to be available
    YASMIN_LOG_INFO("Waiting for action '%s'", this->action_name.c_str());
 
    while (!this->action_client->wait_for_action_server(
        std::chrono::duration<int64_t, std::ratio<1>>(this->wait_timeout))) {
      YASMIN_LOG_WARN("Timeout reached, action '%s' is not available",
                      this->action_name.c_str());
      if (retry_count < this->maximum_retry) {
        retry_count++;
        YASMIN_LOG_WARN("Retrying to connect to action '%s' "
                        "(%d/%d)",
                        this->action_name.c_str(), retry_count,
                        this->maximum_retry);
      } else {
        return basic_outcomes::TIMEOUT;
      }
    }
 
    // Prepare options for sending the goal
    SendGoalOptions send_goal_options;
    send_goal_options.goal_response_callback =
        std::bind(&ActionState::goal_response_callback, this, _1);
 
    send_goal_options.result_callback =
        std::bind(&ActionState::result_callback, this, _1);
 
    if (this->feedback_handler) {
      send_goal_options.feedback_callback =
          [this, blackboard](typename GoalHandle::SharedPtr,
                             std::shared_ptr<const Feedback> feedback) {
            this->feedback_handler(blackboard, feedback);
          };
    }
 
    YASMIN_LOG_INFO("Sending goal to action '%s'", this->action_name.c_str());
    this->action_client->async_send_goal(goal, send_goal_options);
 
    if (this->response_timeout > 0) {
      while (this->action_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 action '%s'",
            this->action_name.c_str());
        if (retry_count < this->maximum_retry) {
          retry_count++;
          YASMIN_LOG_WARN("Retrying to wait for action '%s' response (%d/%d)",
                          this->action_name.c_str(), retry_count,
                          this->maximum_retry);
        } else {
          return basic_outcomes::TIMEOUT;
        }
      }
    } else {
      this->action_done_cond.wait(lock);
    }
 
    if (this->is_canceled()) {
      return basic_outcomes::CANCEL;
    }
 
    switch (this->action_status) {
    case rclcpp_action::ResultCode::CANCELED:
      return basic_outcomes::CANCEL;
 
    case rclcpp_action::ResultCode::ABORTED:
      return basic_outcomes::ABORT;
 
    case rclcpp_action::ResultCode::SUCCEEDED:
      if (this->result_handler) {
        return this->result_handler(blackboard, this->action_result);
      }
      return basic_outcomes::SUCCEED;
 
    default:
      return basic_outcomes::ABORT;
    }
  }
 
protected:
  rclcpp::Node::SharedPtr node_;
 
private:
  std::string action_name;

  ActionClient action_client;

  std::condition_variable action_done_cond;
  std::mutex action_done_mutex;
  std::condition_variable action_cancel_cond;
  std::mutex action_cancel_mutex;

  Result action_result;
  rclcpp_action::ResultCode action_status;

  std::shared_ptr<GoalHandle> goal_handle;
  std::mutex goal_handle_mutex;

  CreateGoalHandler create_goal_handler;
  ResultHandler result_handler;
  FeedbackHandler feedback_handler;

  int wait_timeout;
  int response_timeout;
  int maximum_retry;
 
#if __has_include("rclcpp/version.h")
#include "rclcpp/version.h"
#if RCLCPP_VERSION_GTE(2, 4, 3) // Greater or equal to latest Foxy
  void
  goal_response_callback(const typename GoalHandle::SharedPtr &goal_handle) {
    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
    this->goal_handle = goal_handle;
  }
#else
  void goal_response_callback(
      std::shared_future<typename GoalHandle::SharedPtr> future) {
    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
    this->goal_handle = future.get();
  }
#endif
#else
  void goal_response_callback(
      std::shared_future<typename GoalHandle::SharedPtr> future) {
    std::lock_guard<std::mutex> lock(this->goal_handle_mutex);
    this->goal_handle = future.get();
  }
#endif

  void result_callback(const typename GoalHandle::WrappedResult &result) {
    this->action_result = result.result;
    this->action_status = result.code;
    this->action_done_cond.notify_one();
  }
};
 
} // namespace yasmin_ros
 
#endif // YASMIN_ROS__ACTION_STATE_HPP