hardware_component_interface.hpp

// Copyright 2025 ros2_control Development Team
//
// 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 HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_
#define HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_
 
#include <fmt/compile.h>
 
#include <limits>
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "control_msgs/msg/hardware_status.hpp"
#include "hardware_interface/component_parser.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/introspection.hpp"
#include "hardware_interface/types/hardware_component_interface_params.hpp"
#include "hardware_interface/types/hardware_component_params.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "hardware_interface/types/lifecycle_state_names.hpp"
#include "hardware_interface/types/trigger_type.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/logger.hpp"
#include "rclcpp/logging.hpp"
#include "rclcpp/node_interfaces/node_clock_interface.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/version.h"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
#include "rclcpp_lifecycle/state.hpp"
#include "realtime_tools/async_function_handler.hpp"
#include "realtime_tools/realtime_publisher.hpp"
#include "realtime_tools/realtime_thread_safe_box.hpp"
 
namespace hardware_interface
{
 
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
  HardwareComponentInterface();
 
 
  HardwareComponentInterface(const HardwareComponentInterface & other) = delete;
 
  HardwareComponentInterface(HardwareComponentInterface && other) = delete;
 
  virtual ~HardwareComponentInterface();
 
 
  CallbackReturn init(const hardware_interface::HardwareComponentParams & params);
 
 
  virtual CallbackReturn init_hardware_status_message(
    control_msgs::msg::HardwareStatus & msg_template);
 
 
  virtual return_type update_hardware_status_message(control_msgs::msg::HardwareStatus & msg);
 
 
  virtual CallbackReturn on_init(
    const hardware_interface::HardwareComponentInterfaceParams & params);
 
 
  virtual rclcpp::NodeOptions define_custom_node_options() const;
 
  virtual std::vector<hardware_interface::InterfaceDescription>
  export_unlisted_state_interface_descriptions();
 
  virtual std::vector<StateInterface::ConstSharedPtr> on_export_state_interfaces();
 
  virtual std::vector<hardware_interface::InterfaceDescription>
  export_unlisted_command_interface_descriptions();
 
  virtual std::vector<CommandInterface::SharedPtr> on_export_command_interfaces();
 
 
  virtual return_type prepare_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces);
 
  // Perform switching to the new command interface.
  virtual return_type perform_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces);
 
 
  HardwareComponentCycleStatus trigger_read(
    const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
 
  HardwareComponentCycleStatus trigger_write(
    const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  const std::string & get_name() const;
 
 
  const std::string & get_group_name() const;
 
 
  const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
 
  void set_lifecycle_state(const rclcpp_lifecycle::State & new_state);
 
  uint8_t get_lifecycle_id() const;
 
 
  virtual bool has_state(const std::string & interface_name) const;
 
 
  virtual const StateInterface::SharedPtr & get_state_interface_handle(
    const std::string & interface_name) const;
 
 
  template <typename T>
  bool set_state(
    const StateInterface::SharedPtr & interface_handle, const T & value, bool wait_until_set)
  {
    if (!interface_handle)
    {
      throw std::runtime_error(
        fmt::format(
          "State interface handle is null in hardware component: {}, while calling set_state "
          "method. This should not happen.",
          info_.name));
    }
    return interface_handle->set_value(value, wait_until_set);
  }
 
 
  template <typename T>
  void set_state(const std::string & interface_name, const T & value)
  {
    std::ignore = set_state(get_state_interface_handle(interface_name), value, true);
  }
 
  template <typename T>
  bool get_state(
    const StateInterface::SharedPtr & interface_handle, T & state, bool wait_until_get) const
  {
    if (!interface_handle)
    {
      throw std::runtime_error(
        fmt::format(
          "State interface handle is null in hardware component: {}, while calling get_state "
          "method. This should not happen.",
          info_.name));
    }
    const bool success = interface_handle->get_value(state, wait_until_get);
    if (!success && wait_until_get)
    {
      throw std::runtime_error(
        fmt::format(
          "Failed to get state value from interface: {} in hardware component: {}. This should "
          "not happen.",
          interface_handle->get_name(), info_.name));
    }
    return success;
  }
 
 
  template <typename T = double>
  T get_state(const std::string & interface_name) const
  {
    T state;
    get_state<T>(get_state_interface_handle(interface_name), state, true);
    return state;
  }
 
 
  virtual bool has_command(const std::string & interface_name) const;
 
 
  virtual const CommandInterface::SharedPtr & get_command_interface_handle(
    const std::string & interface_name) const;
 
 
  template <typename T>
  bool set_command(
    const CommandInterface::SharedPtr & interface_handle, const T & value, bool wait_until_set)
  {
    if (!interface_handle)
    {
      throw std::runtime_error(
        fmt::format(
          "Command interface handle is null in hardware component: {}, while calling set_command "
          "method. This should not happen.",
          info_.name));
    }
    return interface_handle->set_value(value, wait_until_set);
  }
 
 
  template <typename T>
  void set_command(const std::string & interface_name, const T & value)
  {
    std::ignore = set_command(get_command_interface_handle(interface_name), value, true);
  }
 
  template <typename T>
  bool get_command(
    const CommandInterface::SharedPtr & interface_handle, T & command, bool wait_until_get) const
  {
    if (!interface_handle)
    {
      throw std::runtime_error(
        fmt::format(
          "Command interface handle is null in hardware component: {}, while calling get_command "
          "method. This should not happen.",
          info_.name));
    }
    const bool success = interface_handle->get_value(command, wait_until_get);
    if (!success && wait_until_get)
    {
      throw std::runtime_error(
        fmt::format(
          "Failed to get command value from interface: {} in hardware component: {}. This should "
          "not happen.",
          interface_handle->get_name(), info_.name));
    }
    return success;
  }
 
 
  template <typename T = double>
  T get_command(const std::string & interface_name) const
  {
    T command;
    get_command<T>(get_command_interface_handle(interface_name), command, true);
    return command;
  }
 
 
  virtual rclcpp::Logger get_logger() const;
 
 
  virtual rclcpp::Clock::SharedPtr get_clock() const;
 
 
  virtual rclcpp::Node::SharedPtr get_node() const;
 
 
  const HardwareInfo & get_hardware_info() const;
 
 
  void pause_async_operations();
 
 
  void stop_async_handler();
 
 
  void prepare_for_activation();
 
 
  void enable_introspection(bool enable);
 
protected:
  HardwareInfo info_;
  // interface names to InterfaceDescription
  std::unordered_map<std::string, InterfaceDescription> joint_state_interfaces_;
  std::unordered_map<std::string, InterfaceDescription> joint_command_interfaces_;
 
  std::unordered_map<std::string, InterfaceDescription> sensor_state_interfaces_;
 
  std::unordered_map<std::string, InterfaceDescription> gpio_state_interfaces_;
  std::unordered_map<std::string, InterfaceDescription> gpio_command_interfaces_;
 
  std::unordered_map<std::string, InterfaceDescription> unlisted_state_interfaces_;
  std::unordered_map<std::string, InterfaceDescription> unlisted_command_interfaces_;
 
  rclcpp_lifecycle::State lifecycle_state_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
 
  // Exported Command- and StateInterfaces in order they are listed in the hardware description.
  std::vector<StateInterface::SharedPtr> joint_states_;
  std::vector<CommandInterface::SharedPtr> joint_commands_;
 
  std::vector<StateInterface::SharedPtr> sensor_states_;
 
  std::vector<StateInterface::SharedPtr> gpio_states_;
  std::vector<CommandInterface::SharedPtr> gpio_commands_;
 
  std::vector<StateInterface::SharedPtr> unlisted_states_;
  std::vector<CommandInterface::SharedPtr> unlisted_commands_;
 
private:
  class HardwareComponentInterfaceImpl;
  std::unique_ptr<HardwareComponentInterfaceImpl> impl_;
 
protected:
  pal_statistics::RegistrationsRAII stats_registrations_;
};
 
}  // namespace hardware_interface
#endif  // HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_