Program Listing for File server_goal_handle.hpp

↰ Return to documentation for file (include/rclcpp_action/server_goal_handle.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_GOAL_HANDLE_HPP_
#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

#include <functional>
#include <memory>
#include <mutex>

#include "rcl_action/types.h"
#include "rcl_action/goal_handle.h"

#include "action_msgs/msg/goal_status.hpp"

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

namespace rclcpp_action
{


class ServerGoalHandleBase
{
public:
  RCLCPP_ACTION_PUBLIC
  bool
  is_canceling() const;

  RCLCPP_ACTION_PUBLIC
  bool
  is_active() const;

  RCLCPP_ACTION_PUBLIC
  bool
  is_executing() const;

  RCLCPP_ACTION_PUBLIC
  virtual
  ~ServerGoalHandleBase();

protected:
  // -------------------------------------------------------------------------
  // API for communication between ServerGoalHandleBase and ServerGoalHandle<>

  RCLCPP_ACTION_PUBLIC
  ServerGoalHandleBase(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle
  )
  : rcl_handle_(rcl_handle)
  {
  }

  RCLCPP_ACTION_PUBLIC
  void
  _abort();

  RCLCPP_ACTION_PUBLIC
  void
  _succeed();

  RCLCPP_ACTION_PUBLIC
  void
  _cancel_goal();

  RCLCPP_ACTION_PUBLIC
  void
  _canceled();

  RCLCPP_ACTION_PUBLIC
  void
  _execute();

  RCLCPP_ACTION_PUBLIC
  bool
  try_canceling() noexcept;

  // End API for communication between ServerGoalHandleBase and ServerGoalHandle<>
  // -----------------------------------------------------------------------------

private:
  std::shared_ptr<rcl_action_goal_handle_t> rcl_handle_;
  mutable std::mutex rcl_handle_mutex_;
};

// Forward declare server
template<typename ActionT>
class Server;


template<typename ActionT>
class ServerGoalHandle : public ServerGoalHandleBase
{
public:

  void
  publish_feedback(std::shared_ptr<typename ActionT::Feedback> feedback_msg)
  {
    auto feedback_message = std::make_shared<typename ActionT::Impl::FeedbackMessage>();
    feedback_message->goal_id.uuid = uuid_;
    feedback_message->feedback = *feedback_msg;
    publish_feedback_(feedback_message);
  }


  void
  abort(typename ActionT::Result::SharedPtr result_msg)
  {
    _abort();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }


  void
  succeed(typename ActionT::Result::SharedPtr result_msg)
  {
    _succeed();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }


  void
  canceled(typename ActionT::Result::SharedPtr result_msg)
  {
    _canceled();
    auto response = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
    response->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
    response->result = *result_msg;
    on_terminal_state_(uuid_, response);
  }


  void
  execute()
  {
    _execute();
    on_executing_(uuid_);
  }

  const std::shared_ptr<const typename ActionT::Goal>
  get_goal() const
  {
    return goal_;
  }

  const GoalUUID &
  get_goal_id() const
  {
    return uuid_;
  }

  virtual ~ServerGoalHandle()
  {
    // Cancel goal if handle was allowed to destruct without reaching a terminal state
    if (try_canceling()) {
      auto null_result = std::make_shared<typename ActionT::Impl::GetResultService::Response>();
      null_result->status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
      on_terminal_state_(uuid_, null_result);
    }
  }

protected:
  ServerGoalHandle(
    std::shared_ptr<rcl_action_goal_handle_t> rcl_handle,
    GoalUUID uuid,
    std::shared_ptr<const typename ActionT::Goal> goal,
    std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state,
    std::function<void(const GoalUUID &)> on_executing,
    std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback
  )
  : ServerGoalHandleBase(rcl_handle), goal_(goal), uuid_(uuid),
    on_terminal_state_(on_terminal_state), on_executing_(on_executing),
    publish_feedback_(publish_feedback)
  {
  }

  const std::shared_ptr<const typename ActionT::Goal> goal_;

  const GoalUUID uuid_;

  friend class Server<ActionT>;

  std::function<void(const GoalUUID &, std::shared_ptr<void>)> on_terminal_state_;
  std::function<void(const GoalUUID &)> on_executing_;
  std::function<void(std::shared_ptr<typename ActionT::Impl::FeedbackMessage>)> publish_feedback_;
};
}  // namespace rclcpp_action

#endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_