Program Listing for File server.hpp

↰ Return to documentation for file (include/rclcpp_action/server.hpp)

// 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 <functional>
#include <memory>
#include <mutex>
#include <string>
#include <unordered_map>
#include <utility>

#include "rcl/event_callback.h"
#include "rcl_action/action_server.h"
#include "rosidl_runtime_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 "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:
  enum class EntityType : std::size_t
  {
    GoalService,
    ResultService,
    CancelService,
  };

  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
  void
  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
  std::shared_ptr<void>
  take_data() override;

  RCLCPP_ACTION_PUBLIC
  std::shared_ptr<void>
  take_data_by_entity_id(size_t id) override;

  RCLCPP_ACTION_PUBLIC
  void
  execute(std::shared_ptr<void> & data) override;


  RCLCPP_ACTION_PUBLIC
  void
  set_on_ready_callback(std::function<void(size_t, int)> callback) override;

  RCLCPP_ACTION_PUBLIC
  void
  clear_on_ready_callback() 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(GoalUUID &, 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 GoalUUID & uuid) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  GoalUUID
  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,
    GoalUUID uuid, std::shared_ptr<void> goal_request_message) = 0;

  RCLCPP_ACTION_PUBLIC
  virtual
  GoalUUID
  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 GoalUUID & 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(std::shared_ptr<void> & data);

  RCLCPP_ACTION_PUBLIC
  void
  execute_cancel_request_received(std::shared_ptr<void> & data);

  RCLCPP_ACTION_PUBLIC
  void
  execute_result_request_received(std::shared_ptr<void> & data);

  RCLCPP_ACTION_PUBLIC
  void
  execute_check_expired_goals();

  std::unique_ptr<ServerBaseImpl> pimpl_;

  RCLCPP_ACTION_PUBLIC
  void
  set_callback_to_entity(
    EntityType entity_type,
    std::function<void(size_t, int)> callback);

protected:
  // Mutex to protect the callbacks storage.
  std::recursive_mutex listener_mutex_;
  // Storage for std::function callbacks to keep them in scope
  std::unordered_map<EntityType, std::function<void(size_t)>> entity_type_to_on_ready_callback_;

  RCLCPP_ACTION_PUBLIC
  void
  set_on_ready_callback(
    EntityType entity_type,
    rcl_event_callback_t callback,
    const void * user_data);

  bool on_ready_callback_set_{false};
};


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 GoalUUID &, 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(GoalUUID & uuid, std::shared_ptr<void> message) override
  {
    auto request = std::static_pointer_cast<
      typename ActionT::Impl::SendGoalService::Request>(message);
    auto goal = std::shared_ptr<typename ActionT::Goal>(request, &request->goal);
    GoalResponse user_response = handle_goal_(uuid, goal);

    auto ros_response = std::make_shared<typename ActionT::Impl::SendGoalService::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 GoalUUID & uuid) override
  {
    std::shared_ptr<ServerGoalHandle<ActionT>> goal_handle;
    {
      std::lock_guard<std::mutex> lock(goal_handles_mutex_);
      auto element = goal_handles_.find(uuid);
      if (element != goal_handles_.end()) {
        goal_handle = element->second.lock();
      }
    }

    CancelResponse resp = CancelResponse::REJECT;
    if (goal_handle) {
      resp = handle_cancel_(goal_handle);
      if (CancelResponse::ACCEPT == resp) {
        try {
          goal_handle->_cancel_goal();
        } catch (const rclcpp::exceptions::RCLError & ex) {
          RCLCPP_DEBUG(
            rclcpp::get_logger("rclcpp_action"),
            "Failed to cancel goal in call_handle_cancel_callback: %s", ex.what());
          return CancelResponse::REJECT;
        }
      }
    }
    return resp;
  }

  void
  call_goal_accepted_callback(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_goal_handle,
    GoalUUID 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 GoalUUID &, std::shared_ptr<void>)> on_terminal_state =
      [weak_this](const GoalUUID & goal_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(goal_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(goal_uuid);
      };

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

    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback =
      [weak_this](std::shared_ptr<typename ActionT::Impl::FeedbackMessage> 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));
      };

    auto request = std::static_pointer_cast<
      const typename ActionT::Impl::SendGoalService::Request>(goal_request_message);
    auto goal = std::shared_ptr<const typename ActionT::Goal>(request, &request->goal);
    goal_handle.reset(
      new ServerGoalHandle<ActionT>(
        rcl_goal_handle, uuid, goal, 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);
  }

  GoalUUID
  get_goal_id_from_goal_request(void * message) override
  {
    return
      static_cast<typename ActionT::Impl::SendGoalService::Request *>(message)->goal_id.uuid;
  }

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

  GoalUUID
  get_goal_id_from_result_request(void * message) override
  {
    return
      static_cast<typename ActionT::Impl::GetResultService::Request *>(message)->goal_id.uuid;
  }

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

  std::shared_ptr<void>
  create_result_response(decltype(action_msgs::msg::GoalStatus::status) status) override
  {
    auto result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    result->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<GoalUUID, GoalHandleWeakPtr> goal_handles_;
  std::mutex goal_handles_mutex_;
};
}  // namespace rclcpp_action
#endif  // RCLCPP_ACTION__SERVER_HPP_