action_client_wrapper.hpp

// Copyright 2024 Robin Müller
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      https://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#pragma once
 
#include <chrono>
 
#include "action_msgs/srv/cancel_goal.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"


namespace auto_apms_util
{

enum class ActionGoalStatus : uint8_t
{
  REJECTED = 0,
  RUNNING,
  COMPLETED
};

template <typename ActionT>
class ActionClientWrapper
{
public:
  using Goal = typename ActionT::Goal;
  using SendGoalOptions = typename rclcpp_action::Client<ActionT>::SendGoalOptions;
  using Feedback = typename ActionT::Feedback;
  using Result = typename ActionT::Result;
  using ClientGoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
  using WrappedResultSharedPtr = std::shared_ptr<typename ClientGoalHandle::WrappedResult>;
  using ResultFuture = std::shared_future<WrappedResultSharedPtr>;

  ActionClientWrapper(rclcpp::Node::SharedPtr node_ptr, const std::string & action_name);

  ResultFuture syncSendGoal(
    const Goal & goal = Goal{}, const SendGoalOptions & options = SendGoalOptions{},
    const std::chrono::seconds server_timeout = std::chrono::seconds{3},
    const std::chrono::seconds response_timeout = std::chrono::seconds{3});

  bool syncCancelLastGoal(const std::chrono::seconds response_timeout = std::chrono::seconds{3});

  static ActionGoalStatus getGoalStatus(const ResultFuture & future);

  typename ClientGoalHandle::SharedPtr getActiveGoalHandle();

  std::shared_ptr<const Feedback> getFeedback();
 
private:
  const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ptr_;
  const rclcpp::Logger logger_;
  const std::string action_name_;
  typename rclcpp_action::Client<ActionT>::SharedPtr client_ptr_;
  typename ClientGoalHandle::SharedPtr goal_handle_ptr_;
  std::shared_ptr<const Feedback> feedback_ptr_;
};
 
template <typename ActionT>
ActionClientWrapper<ActionT>::ActionClientWrapper(rclcpp::Node::SharedPtr node_ptr, const std::string & action_name)
: node_base_interface_ptr_{node_ptr->get_node_base_interface()},
  logger_{node_ptr->get_logger()},
  action_name_{action_name}
{
  client_ptr_ = rclcpp_action::create_client<ActionT>(node_ptr, action_name_);
}
 
template <typename ActionT>
typename ActionClientWrapper<ActionT>::ResultFuture ActionClientWrapper<ActionT>::syncSendGoal(
  const Goal & goal, const SendGoalOptions & options, const std::chrono::seconds server_timeout,
  const std::chrono::seconds response_timeout)
{
  auto promise_ptr = std::make_shared<std::promise<WrappedResultSharedPtr>>();
  if (!client_ptr_->wait_for_action_server(server_timeout)) {
    throw std::runtime_error("Action server '" + action_name_ + "' is not available");
  }
 
  SendGoalOptions _options;
  _options.goal_response_callback = options.goal_response_callback;
  _options.feedback_callback = [this, feedback_callback = options.feedback_callback](
                                 typename ClientGoalHandle::SharedPtr client_goal_handle,
                                 const std::shared_ptr<const Feedback> feedback) {
    feedback_ptr_ = feedback;
    if (feedback_callback) {
      feedback_callback(client_goal_handle, feedback);
    }
  };
  _options.result_callback = [this, promise_ptr, result_callback = options.result_callback](
                               const typename ClientGoalHandle::WrappedResult & wr) {
    if (result_callback) {
      result_callback(wr);
    }
    promise_ptr->set_value(std::make_shared<typename ClientGoalHandle::WrappedResult>(wr));
    goal_handle_ptr_ = nullptr;  // Reset active goal handle
  };
 
  auto goal_response_future = client_ptr_->async_send_goal(goal, _options);
 
  switch (rclcpp::spin_until_future_complete(node_base_interface_ptr_, goal_response_future, response_timeout)) {
    case rclcpp::FutureReturnCode::SUCCESS:
      break;
    case rclcpp::FutureReturnCode::TIMEOUT:
      throw std::runtime_error("No goal response due to timeout");
    case rclcpp::FutureReturnCode::INTERRUPTED:
      throw std::runtime_error("No goal response due to interruption");
  }
 
  auto client_goal_handle_ptr = goal_response_future.get();
  if (!client_goal_handle_ptr) {
    promise_ptr->set_value(nullptr);
    return promise_ptr->get_future();
  }
 
  goal_handle_ptr_ = client_goal_handle_ptr;
  return promise_ptr->get_future();
}
 
template <typename ActionT>
bool ActionClientWrapper<ActionT>::syncCancelLastGoal(const std::chrono::seconds response_timeout)
{
  if (!goal_handle_ptr_) {
    throw std::runtime_error("Cannot cancel goal because goal_handle_ptr_ is nullptr");
  }
 
  // Send request and await response
  auto cancel_response_future = client_ptr_->async_cancel_goal(goal_handle_ptr_);
  switch (rclcpp::spin_until_future_complete(node_base_interface_ptr_, cancel_response_future, response_timeout)) {
    case rclcpp::FutureReturnCode::SUCCESS:
      break;
    case rclcpp::FutureReturnCode::TIMEOUT:
      throw std::runtime_error("No cancel response due to timeout");
    case rclcpp::FutureReturnCode::INTERRUPTED:
      throw std::runtime_error("No cancel response due to interruption");
  }
 
  // Evaluate the response message
  switch (cancel_response_future.get()->return_code) {
    case action_msgs::srv::CancelGoal::Response::ERROR_NONE:
      return true;
    case action_msgs::srv::CancelGoal::Response::ERROR_REJECTED:
      break;
    case action_msgs::srv::CancelGoal::Response::ERROR_UNKNOWN_GOAL_ID:
      RCLCPP_WARN(logger_, "Cancel response ERROR_UNKNOWN_GOAL_ID");
      break;
    case action_msgs::srv::CancelGoal::Response::ERROR_GOAL_TERMINATED:
      RCLCPP_WARN(logger_, "Cancel response ERROR_GOAL_TERMINATED");
      break;
  }
  return false;
}
 
template <typename ActionT>
ActionGoalStatus ActionClientWrapper<ActionT>::getGoalStatus(const ResultFuture & future)
{
  if (!future.valid()) {
    throw std::runtime_error("ResultFuture is not valid");
  }
  if (future.wait_for(std::chrono::seconds(0)) == std::future_status::ready) {
    if (!future.get()) {
      return ActionGoalStatus::REJECTED;
    }
    return ActionGoalStatus::COMPLETED;
  }
  return ActionGoalStatus::RUNNING;
}
 
template <typename ActionT>
typename ActionClientWrapper<ActionT>::ClientGoalHandle::SharedPtr ActionClientWrapper<ActionT>::getActiveGoalHandle()
{
  return goal_handle_ptr_;
}
 
template <typename ActionT>
std::shared_ptr<const typename ActionClientWrapper<ActionT>::Feedback> ActionClientWrapper<ActionT>::getFeedback()
{
  return feedback_ptr_;
}
 
}  // namespace auto_apms_util