action_server_base.hpp

// Copyright 2026 Intelligent Robotics Lab
//
// This file is part of the project EasyFleet
// 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 EASYFLEET_CORE__ACTION_SERVER_BASE_HPP_
#define EASYFLEET_CORE__ACTION_SERVER_BASE_HPP_
 
#include <atomic>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
 
namespace easyfleet_core
{


template<typename ActionT>
class ActionServerBase
{
public:
  using ActionType = ActionT;
  using Goal = typename ActionT::Goal;
  using Feedback = typename ActionT::Feedback;
  using Result = typename ActionT::Result;
  using GoalHandle = rclcpp_action::ServerGoalHandle<ActionT>;
  using GoalHandleSharedPtr = std::shared_ptr<GoalHandle>;
  using SharedPtr = std::shared_ptr<ActionServerBase<ActionT>>;
 
  ActionServerBase(const ActionServerBase &) = delete;
  ActionServerBase & operator=(const ActionServerBase &) = delete;
 
  virtual ~ActionServerBase();

  const std::string & get_action_name() const noexcept;

  bool is_active() const;

  bool is_preemptable() const noexcept;
 
protected:
  template<typename NodeT>
  explicit ActionServerBase(
    NodeT & node,
    const std::string & action_name,
    bool default_allow_preemption = true)
  : ActionServerBase(
      node.get_node_base_interface(),
      node.get_node_clock_interface(),
      node.get_node_logging_interface(),
      node.get_node_parameters_interface(),
      node.get_node_waitables_interface(),
      action_name,
      default_allow_preemption)
  {
  }

  virtual rclcpp_action::GoalResponse on_goal_received(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Goal> goal) = 0;

  virtual void on_execute(const GoalHandleSharedPtr goal_handle) = 0;

  virtual rclcpp_action::CancelResponse on_cancel_requested(const GoalHandleSharedPtr goal_handle);

  virtual void on_preempted(const GoalHandleSharedPtr & preempted_goal_handle);

  bool is_preempt_requested() const noexcept;

  bool is_shutdown_requested() const noexcept;

  rclcpp::Logger logger() const;
 
private:
  ActionServerBase(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
    rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables,
    const std::string & action_name,
    bool default_allow_preemption);
 
  rclcpp_action::GoalResponse handle_goal(
    const rclcpp_action::GoalUUID & uuid,
    std::shared_ptr<const Goal> goal);
  rclcpp_action::CancelResponse handle_cancel(const GoalHandleSharedPtr goal_handle);
  void handle_accepted(GoalHandleSharedPtr goal_handle);
  void on_parameters_set(const std::vector<rclcpp::Parameter> & params);
  void worker_loop();
 
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
  rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters_;
  rclcpp::node_interfaces::NodeWaitablesInterface::SharedPtr node_waitables_;
 
  std::string action_name_;
  std::string robot_name_;
  std::string param_name_;
  std::atomic_bool allow_preemption_{true};
 
  typename rclcpp_action::Server<ActionT>::SharedPtr server_;
  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr param_cb_handle_;
 
  mutable std::mutex mutex_;
  std::condition_variable cv_;
  std::atomic_bool shutting_down_{false};
  std::atomic_bool preempt_requested_{false};
  GoalHandleSharedPtr active_handle_;
  GoalHandleSharedPtr pending_handle_;
  std::thread worker_;
};
 
}  // namespace easyfleet_core
 
#include "easyfleet_core/detail/action_server_base_impl.hpp"
 
#endif  // EASYFLEET_CORE__ACTION_SERVER_BASE_HPP_