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
  _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 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_