server_goal_handle.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_GOAL_HANDLE_HPP_
#define RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_

#include <rcl_action/types.h>
#include <rcl_action/goal_handle.h>

#include <action_msgs/msg/goal_status.hpp>

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

#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
  _set_aborted();

  RCLCPP_ACTION_PUBLIC
  void
  _set_succeeded();

  RCLCPP_ACTION_PUBLIC
  void
  _set_canceling();

  RCLCPP_ACTION_PUBLIC
  void
  _set_canceled();

  RCLCPP_ACTION_PUBLIC
  void
  _set_executing();

  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)
  {
    feedback_msg->action_goal_id.uuid = uuid_;
    publish_feedback_(feedback_msg);
  }

  // TODO(sloretz) which exception is raised?

  void
  set_aborted(typename ActionT::Result::SharedPtr result_msg)
  {
    _set_aborted();
    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_ABORTED;
    on_terminal_state_(uuid_, result_msg);
  }


  void
  set_succeeded(typename ActionT::Result::SharedPtr result_msg)
  {
    _set_succeeded();
    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_SUCCEEDED;
    on_terminal_state_(uuid_, result_msg);
  }


  void
  set_canceled(typename ActionT::Result::SharedPtr result_msg)
  {
    _set_canceled();
    result_msg->action_status = action_msgs::msg::GoalStatus::STATUS_CANCELED;
    on_terminal_state_(uuid_, result_msg);
  }


  void
  set_executing()
  {
    _set_executing();
    on_executing_(uuid_);
  }

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

  const GoalID &
  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::Result>();
      null_result->action_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,
    GoalID uuid,
    std::shared_ptr<const typename ActionT::Goal> goal,
    std::function<void(const GoalID &, std::shared_ptr<void>)> on_terminal_state,
    std::function<void(const GoalID &)> on_executing,
    std::function<void(std::shared_ptr<typename ActionT::Feedback>)> 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 GoalID uuid_;

  friend Server<ActionT>;

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

#endif  // RCLCPP_ACTION__SERVER_GOAL_HANDLE_HPP_