capability.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__CAPABILITY_HPP_
#define EASYFLEET_CORE__CAPABILITY_HPP_
 
#include <memory>
#include <string>
 
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 
#include "easyfleet_core/action_server_base.hpp"
#include "easyfleet_core/capability_node_base.hpp"
#include "easyfleet_interfaces/msg/capability_description.hpp"
#include "easyfleet_interfaces/msg/capability_status.hpp"
 
namespace easyfleet_core
{


template<typename ActionServerT>
class Capability : public rclcpp_lifecycle::LifecycleNode, public CapabilityNodeBase
{
public:
  using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
  using ActionServerBaseT = ActionServerBase<typename ActionServerT::ActionType>;

  explicit Capability(
    const std::string & capability_name,
    const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
  ~Capability() override;

  typename ActionServerBaseT::SharedPtr get_action_server() const noexcept;

  const std::string & get_capability_name() const noexcept override;

  const std::string & get_robot_name() const noexcept override;

  const std::string & get_resolved_action_name() const noexcept;
 
  // -- CapabilityNodeBase --
  //
  // Named *_node() (see capability_node_base.hpp) precisely so these don't
  // collide with rclcpp_lifecycle::LifecycleNode's own same-purpose
  // configure()/activate()/.../shutdown() member functions -- those keep
  // working completely unchanged (e.g. test_capability.cpp's
  // `capability_->configure(cb)` one-arg overload) since nothing here
  // hides or shadows them. Each *_node() implementation (capability_impl.hpp)
  // just calls the corresponding LifecycleNode transition and returns its
  // CallbackReturn. get_node_base_interface() is the one exception: its
  // name/signature/return type happen to exactly match LifecycleNode's own,
  // so this override *does* hide that one -- harmlessly, since it forwards
  // to it internally and nothing in this codebase calls it on a
  // `Capability<T>` directly today.
  CallbackReturn configure_node() override;
  CallbackReturn activate_node() override;
  CallbackReturn deactivate_node() override;
  CallbackReturn cleanup_node() override;
  CallbackReturn shutdown_node() override;
  uint8_t get_current_state_id() const override;
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() override;
 
protected:
  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
  CallbackReturn on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
 
private:
  void publish_heartbeat();
  void stop_heartbeat();
 
  std::string capability_name_;
  std::string robot_name_;
  std::string resolved_action_name_;
  typename ActionServerBaseT::SharedPtr action_server_;
 
  rclcpp_lifecycle::LifecyclePublisher<
    easyfleet_interfaces::msg::CapabilityDescription>::SharedPtr capabilities_pub_;
  rclcpp_lifecycle::LifecyclePublisher<
    easyfleet_interfaces::msg::CapabilityStatus>::SharedPtr status_pub_;
  rclcpp::TimerBase::SharedPtr heartbeat_timer_;
};
 
}  // namespace easyfleet_core
 
#include "easyfleet_core/detail/capability_impl.hpp"
 
#endif  // EASYFLEET_CORE__CAPABILITY_HPP_