controller_interface_base.hpp

// Copyright (c) 2022, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// 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 CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
#define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_
 
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>
 
#include "realtime_tools/async_function_handler.hpp"
 
#include "controller_interface/controller_interface_params.hpp"
#include "hardware_interface/handle.hpp"
#include "hardware_interface/introspection.hpp"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
 
#include "lifecycle_msgs/msg/state.hpp"
#include "rclcpp/version.h"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
namespace controller_interface
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
enum class return_type : std::uint8_t
{
  OK = 0,
  ERROR = 1,
};
 
 
enum class interface_configuration_type : std::uint8_t
{
  ALL = 0,
  INDIVIDUAL = 1,
  NONE = 2,
};
 
struct InterfaceConfiguration
{
  interface_configuration_type type;
  std::vector<std::string> names = {};
};
 
struct ControllerUpdateStats
{
  void reset()
  {
    total_triggers = 0;
    failed_triggers = 0;
  }
 
  unsigned int total_triggers;
  unsigned int failed_triggers;
};
 
struct ControllerUpdateStatus
{
  bool successful = true;
  return_type result = return_type::OK;
  std::optional<std::chrono::nanoseconds> execution_time = std::nullopt;
  std::optional<rclcpp::Duration> period = std::nullopt;
};
 
class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface
{
public:
  ControllerInterfaceBase() = default;
 
  virtual ~ControllerInterfaceBase();
 
 
  virtual InterfaceConfiguration command_interface_configuration() const = 0;
 
 
  virtual InterfaceConfiguration state_interface_configuration() const = 0;
 
 
  virtual void assign_interfaces(
    std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
    std::vector<hardware_interface::LoanedStateInterface> && state_interfaces);
 
 
  virtual void release_interfaces();
 
  return_type init(
    const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate,
    const std::string & node_namespace, const rclcpp::NodeOptions & node_options);
 
  return_type init(const controller_interface::ControllerInterfaceParams & params);
 
  /*
   * Override default implementation for configure of LifecycleNode to get parameters.
   */
  const rclcpp_lifecycle::State & configure();
 
  virtual CallbackReturn on_init() = 0;
 
  virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
  ControllerUpdateStatus trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period);
 
  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
 
  std::shared_ptr<const rclcpp_lifecycle::LifecycleNode> get_node() const;
 
  const rclcpp_lifecycle::State & get_lifecycle_state() const;
 
  uint8_t get_lifecycle_id() const;
 
  unsigned int get_update_rate() const;
 
  bool is_async() const;
 
  const std::string & get_robot_description() const;
 
  const std::unordered_map<std::string, joint_limits::JointLimits> & get_hard_joint_limits() const;
 
  const std::unordered_map<std::string, joint_limits::SoftJointLimits> & get_soft_joint_limits()
    const;
 
  virtual rclcpp::NodeOptions define_custom_node_options() const
  {
    rclcpp::NodeOptions node_options;
// \note The versions conditioning is added here to support the source-compatibility with Humble
#if RCLCPP_VERSION_MAJOR >= 21
    node_options.enable_logger_service(true);
#else
    node_options.allow_undeclared_parameters(true);
    node_options.automatically_declare_parameters_from_overrides(true);
#endif
    return node_options;
  }
 
 
  template <typename ParameterT>
  auto auto_declare(const std::string & name, const ParameterT & default_value)
  {
    if (!node_->has_parameter(name))
    {
      return node_->declare_parameter<ParameterT>(name, default_value);
    }
    else
    {
      return node_->get_parameter(name).get_value<ParameterT>();
    }
  }
 
  // Methods for chainable controller types with default values so we can put all controllers into
  // one list in Controller Manager
 
 
  virtual bool is_chainable() const = 0;
 
  virtual std::vector<hardware_interface::CommandInterface::SharedPtr>
  export_reference_interfaces() = 0;
 
  virtual std::vector<hardware_interface::StateInterface::ConstSharedPtr>
  export_state_interfaces() = 0;
 
  virtual bool set_chained_mode(bool chained_mode) = 0;
 
 
  virtual bool is_in_chained_mode() const = 0;
 
  void wait_for_trigger_update_to_finish();
 
  void prepare_for_deactivation();
 
  std::string get_name() const;
 
 
  void enable_introspection(bool enable);
 
protected:
  std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
  std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
 
private:
  void stop_async_handler_thread();
 
  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
  std::unique_ptr<realtime_tools::AsyncFunctionHandler<return_type>> async_handler_;
  bool is_async_ = false;
  controller_interface::ControllerInterfaceParams ctrl_itf_params_;
  std::atomic_bool skip_async_triggers_ = false;
  ControllerUpdateStats trigger_stats_;
  mutable std::atomic<uint8_t> lifecycle_id_ = lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN;
 
protected:
  pal_statistics::RegistrationsRAII stats_registrations_;
};
 
using ControllerInterfaceBaseSharedPtr = std::shared_ptr<ControllerInterfaceBase>;
 
}  // namespace controller_interface
 
#endif  // CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_