server.hpp

Go to the documentation of this file.

// Copyright 2018 Open Source Robotics Foundation, Inc.
//
// 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
//
//     http://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.

#ifndef RCLCPP_ACTION__SERVER_HPP_
#define RCLCPP_ACTION__SERVER_HPP_

#include <rcl_action/action_server.h>
#include <rosidl_generator_c/action_type_support_struct.h>
#include <rosidl_typesupport_cpp/action_type_support.hpp>
#include <rclcpp/node_interfaces/node_base_interface.hpp>
#include <rclcpp/node_interfaces/node_clock_interface.hpp>
#include <rclcpp/node_interfaces/node_logging_interface.hpp>
#include <rclcpp/waitable.hpp>

#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <utility>

#include "rclcpp_action/visibility_control.hpp"
#include "rclcpp_action/server_goal_handle.hpp"
#include "rclcpp_action/types.hpp"

namespace rclcpp_action
{
// Forward declaration
class ServerBaseImpl;

enum class GoalResponse : int8_t
{
  REJECT = 1,
  ACCEPT_AND_EXECUTE = 2,
  ACCEPT_AND_DEFER = 3,
};

enum class CancelResponse : int8_t
{
  REJECT = 1,
  ACCEPT = 2,
};


class ServerBase : public rclcpp::Waitable
{
public:
  RCLCPP_ACTION_PUBLIC
  virtual ~ServerBase();

  // -------------
  // Waitables API

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_subscriptions() override;

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_timers() override;

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_clients() override;

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_services() override;

  RCLCPP_ACTION_PUBLIC
  size_t
  get_number_of_ready_guard_conditions() override;

  RCLCPP_ACTION_PUBLIC
  bool
  add_to_wait_set(rcl_wait_set_t * wait_set) override;

  RCLCPP_ACTION_PUBLIC
  bool
  is_ready(rcl_wait_set_t *) override;

  RCLCPP_ACTION_PUBLIC
  void
  execute() override;

  // End Waitables API
  // -----------------

protected:
  RCLCPP_ACTION_PUBLIC
  ServerBase(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    const std::string & name,
    const rosidl_action_type_support_t * type_support,
    const rcl_action_server_options_t & options);

  // -----------------------------------------------------
  // API for communication between ServerBase and Server<>

  // ServerBase will call this function when a goal request is received.
  // The subclass should convert to the real type and call a user's callback.
  RCLCPP_ACTION_PUBLIC
  virtual
  std::pair<GoalResponse, std::shared_ptr<void>>
  call_handle_goal_callback(GoalID &, std::shared_ptr<void> request) = 0;

  // ServerBase will determine which goal ids are being cancelled, and then call this function for
  // each goal id.
  // The subclass should look up a goal handle and call the user's callback.
  RCLCPP_ACTION_PUBLIC
  virtual
  CancelResponse
  call_handle_cancel_callback(const GoalID & uuid) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  GoalID
  get_goal_id_from_goal_request(void * message) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_goal_request() = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  void
  call_goal_accepted_callback(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
    GoalID uuid, std::shared_ptr<void> goal_request_message) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  GoalID
  get_goal_id_from_result_request(void * message) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_result_request() = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  std::shared_ptr<void>
  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) = 0;

  RCLCPP_ACTION_PUBLIC
  void
  publish_status();

  RCLCPP_ACTION_PUBLIC
  void
  notify_goal_terminal_state();

  RCLCPP_ACTION_PUBLIC
  void
  publish_result(const GoalID & uuid, std::shared_ptr<void> result_msg);

  RCLCPP_ACTION_PUBLIC
  void
  publish_feedback(std::shared_ptr<void> feedback_msg);

  // End API for communication between ServerBase and Server<>
  // ---------------------------------------------------------

private:
  RCLCPP_ACTION_PUBLIC
  void
  execute_goal_request_received();

  RCLCPP_ACTION_PUBLIC
  void
  execute_cancel_request_received();

  RCLCPP_ACTION_PUBLIC
  void
  execute_result_request_received();

  RCLCPP_ACTION_PUBLIC
  void
  execute_check_expired_goals();

  std::unique_ptr<ServerBaseImpl> pimpl_;
};


template<typename ActionT>
class Server : public ServerBase, public std::enable_shared_from_this<Server<ActionT>>
{
public:
  RCLCPP_SMART_PTR_DEFINITIONS_NOT_COPYABLE(Server)

  
  using GoalCallback = std::function<GoalResponse(
        const GoalID &, std::shared_ptr<const typename ActionT::Goal>)>;
  using CancelCallback = std::function<CancelResponse(std::shared_ptr<ServerGoalHandle<ActionT>>)>;
  using AcceptedCallback = std::function<void (std::shared_ptr<ServerGoalHandle<ActionT>>)>;


  Server(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    const std::string & name,
    const rcl_action_server_options_t & options,
    GoalCallback handle_goal,
    CancelCallback handle_cancel,
    AcceptedCallback handle_accepted
  )
  : ServerBase(
      node_base,
      node_clock,
      node_logging,
      name,
      rosidl_typesupport_cpp::get_action_type_support_handle<ActionT>(),
      options),
    handle_goal_(handle_goal),
    handle_cancel_(handle_cancel),
    handle_accepted_(handle_accepted)
  {
  }

  virtual ~Server() = default;

protected:
  // -----------------------------------------------------
  // API for communication between ServerBase and Server<>

  std::pair<GoalResponse, std::shared_ptr<void>>
  call_handle_goal_callback(GoalID & uuid, std::shared_ptr<void> message) override
  {
    // TODO(sloretz) update and remove assert when IDL pipeline allows nesting user's type
    static_assert(
      std::is_same<typename ActionT::Goal, typename ActionT::GoalRequestService::Request>::value,
      "Assuming user fields were merged with goal request fields");
    GoalResponse user_response = handle_goal_(
      uuid, std::static_pointer_cast<typename ActionT::Goal>(message));

    auto ros_response = std::make_shared<typename ActionT::GoalRequestService::Response>();
    ros_response->accepted = GoalResponse::ACCEPT_AND_EXECUTE == user_response ||
      GoalResponse::ACCEPT_AND_DEFER == user_response;
    return std::make_pair(user_response, ros_response);
  }

  CancelResponse
  call_handle_cancel_callback(const GoalID & uuid) override
  {
    std::lock_guard<std::mutex> lock(goal_handles_mutex_);
    CancelResponse resp = CancelResponse::REJECT;
    auto element = goal_handles_.find(uuid);
    if (element != goal_handles_.end()) {
      std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle = element->second.lock();
      if (goal_handle) {
        resp = handle_cancel_(goal_handle);
        if (CancelResponse::ACCEPT == resp) {
          goal_handle->_set_canceling();
        }
      }
    }
    return resp;
  }

  void
  call_goal_accepted_callback(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
    GoalID uuid, std::shared_ptr<void> goal_request_message) override
  {
    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
    std::weak_ptr<Server<ActionT>> weak_this = this->shared_from_this();

    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state =
      [weak_this](const GoalID & uuid, std::shared_ptr<void> result_message)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        // Send result message to anyone that asked
        shared_this->publish_result(uuid, result_message);
        // Publish a status message any time a goal handle changes state
        shared_this->publish_status();
        // notify base so it can recalculate the expired goal timer
        shared_this->notify_goal_terminal_state();
        // Delete data now (ServerBase and rcl_action_server_t keep data until goal handle expires)
        std::lock_guard<std::mutex> lock(shared_this->goal_handles_mutex_);
        shared_this->goal_handles_.erase(uuid);
      };

    std::function<void(const GoalID &)> on_executing =
      [weak_this](const GoalID & uuid)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        (void)uuid;
        // Publish a status message any time a goal handle changes state
        shared_this->publish_status();
      };

    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> publish_feedback =
      [weak_this](std::shared_ptr<typename ActionT::Feedback> feedback_msg)
      {
        std::shared_ptr<Server<ActionT>> shared_this = weak_this.lock();
        if (!shared_this) {
          return;
        }
        shared_this->publish_feedback(std::static_pointer_cast<void>(feedback_msg));
      };

    goal_handle.reset(
      new ServerGoalHandle<ActionT>(
        rcl_goal_handle, uuid,
        std::static_pointer_cast<const typename ActionT::Goal>(goal_request_message),
        on_terminal_state, on_executing, publish_feedback));
    {
      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
      goal_handles_[uuid] = goal_handle;
    }
    handle_accepted_(goal_handle);
  }

  GoalID
  get_goal_id_from_goal_request(void * message) override
  {
    return
      static_cast<typename ActionT::GoalRequestService::Request *>(message)->action_goal_id.uuid;
  }

  std::shared_ptr<void>
  create_goal_request() override
  {
    return std::shared_ptr<void>(new typename ActionT::GoalRequestService::Request());
  }

  GoalID
  get_goal_id_from_result_request(void * message) override
  {
    return
      static_cast<typename ActionT::GoalResultService::Request *>(message)->action_goal_id.uuid;
  }

  std::shared_ptr<void>
  create_result_request() override
  {
    return std::shared_ptr<void>(new typename ActionT::GoalResultService::Request());
  }

  std::shared_ptr<void>
  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
  {
    auto result = std::make_shared<typename ActionT::GoalResultService::Response>();
    result->action_status = status;
    return std::static_pointer_cast<void>(result);
  }

  // End API for communication between ServerBase and Server<>
  // ---------------------------------------------------------

private:
  GoalCallback handle_goal_;
  CancelCallback handle_cancel_;
  AcceptedCallback handle_accepted_;

  using GoalHandleWeakPtr = std::weak_ptr<ServerGoalHandle<ActionT>>;
  std::unordered_map<GoalID, GoalHandleWeakPtr> goal_handles_;
  std::mutex goal_handles_mutex_;
};
}  // namespace rclcpp_action
#endif  // RCLCPP_ACTION__SERVER_HPP_