resource_manager.hpp

// Copyright 2020 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 HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
#define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
 
#include <memory>
#include <string>
#include <unordered_map>
#include <vector>
 
#include "hardware_interface/actuator.hpp"
#include "hardware_interface/hardware_component_info.hpp"
#include "hardware_interface/hardware_info.hpp"
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include "hardware_interface/loaned_command_interface.hpp"
#include "hardware_interface/loaned_state_interface.hpp"
#pragma GCC diagnostic pop
#include "hardware_interface/sensor.hpp"
#include "hardware_interface/system.hpp"
#include "hardware_interface/system_interface.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "rclcpp/duration.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/time.hpp"
 
namespace hardware_interface
{
class ResourceStorage;
class ControllerManager;
 
struct HardwareReadWriteStatus
{
  bool ok;
  std::vector<std::string> failed_hardware_names;
};
 
class ResourceManager
{
public:
  explicit ResourceManager(
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface);
 
  explicit ResourceManager(rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger);
 
 
  explicit ResourceManager(
    const std::string & urdf,
    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
    bool activate_all = false, const unsigned int update_rate = 100);
 
 
  explicit ResourceManager(
    const std::string & urdf, rclcpp::Clock::SharedPtr clock, rclcpp::Logger logger,
    bool activate_all = false, const unsigned int update_rate = 100);
 
  ResourceManager(const ResourceManager &) = delete;
 
  virtual ~ResourceManager();
 
 
  bool shutdown_components();
 
 
  virtual bool load_and_initialize_components(
    const std::string & urdf, const unsigned int update_rate = 100);
 
  void import_joint_limiters(const std::string & urdf);
 
  bool are_components_initialized() const;
 
 
  LoanedStateInterface claim_state_interface(const std::string & key);
 
 
  std::vector<std::string> state_interface_keys() const;
 
 
  std::vector<std::string> available_state_interfaces() const;
 
 
  bool state_interface_is_available(const std::string & name) const;
 
 
  std::string get_state_interface_data_type(const std::string & name) const;
 
 
  void import_controller_exported_state_interfaces(
    const std::string & controller_name, std::vector<StateInterface::ConstSharedPtr> & interfaces);
 
 
  std::vector<std::string> get_controller_exported_state_interface_names(
    const std::string & controller_name);
 
 
  void make_controller_exported_state_interfaces_available(const std::string & controller_name);
 
 
  void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name);
 
 
  void remove_controller_exported_state_interfaces(const std::string & controller_name);
 
 
  void import_controller_reference_interfaces(
    const std::string & controller_name,
    const std::vector<hardware_interface::CommandInterface::SharedPtr> & interfaces);
 
 
  std::vector<std::string> get_controller_reference_interface_names(
    const std::string & controller_name);
 
 
  void make_controller_reference_interfaces_available(const std::string & controller_name);
 
 
  void make_controller_reference_interfaces_unavailable(const std::string & controller_name);
 
 
  void remove_controller_reference_interfaces(const std::string & controller_name);
 
 
  void cache_controller_to_hardware(
    const std::string & controller_name, const std::vector<std::string> & interfaces);
 
 
  std::vector<std::string> get_cached_controllers_to_hardware(const std::string & hardware_name);
 
 
  bool command_interface_is_claimed(const std::string & key) const;
 
 
  LoanedCommandInterface claim_command_interface(const std::string & key);
 
 
  std::vector<std::string> command_interface_keys() const;
 
 
  std::vector<std::string> available_command_interfaces() const;
 
 
  bool command_interface_is_available(const std::string & interface) const;
 
 
  std::string get_command_interface_data_type(const std::string & name) const;
 
 
  size_t actuator_components_size() const;
 
 
  size_t sensor_components_size() const;
 
 
  size_t system_components_size() const;
 
 
  void import_component(
    std::unique_ptr<ActuatorInterface> actuator, const HardwareInfo & hardware_info);
 
 
  void import_component(
    std::unique_ptr<SensorInterface> sensor, const HardwareInfo & hardware_info);
 
 
  void import_component(
    std::unique_ptr<SystemInterface> system, const HardwareInfo & hardware_info);
 
 
  const std::unordered_map<std::string, HardwareComponentInfo> & get_components_status();
 
 
  bool prepare_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces);
 
 
  bool perform_command_mode_switch(
    const std::vector<std::string> & start_interfaces,
    const std::vector<std::string> & stop_interfaces);
 
 
  return_type set_component_state(
    const std::string & component_name, rclcpp_lifecycle::State & target_state);
 
  bool enforce_command_limits(const rclcpp::Duration & period);
 
 
  HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period);
 
 
  bool command_interface_exists(const std::string & key) const;
 
 
  bool state_interface_exists(const std::string & key) const;
 
 
  void set_on_component_state_switch_callback(std::function<void()> callback);
 
  const std::string & get_robot_description() const;
 
protected:
 
  rclcpp::Logger get_logger() const;
 
 
  rclcpp::Clock::SharedPtr get_clock() const;
 
  bool components_are_loaded_and_initialized_ = false;
 
  mutable std::recursive_mutex resource_interfaces_lock_;
  mutable std::recursive_mutex claimed_command_interfaces_lock_;
  mutable std::recursive_mutex resources_lock_;
  mutable std::recursive_mutex joint_limiters_lock_;
 
private:
  bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
  void release_command_interface(const std::string & key);
 
  std::unordered_map<std::string, bool> claimed_command_interface_map_;
 
  std::unique_ptr<ResourceStorage> resource_storage_;
 
  // Structure to store read and write status so it is not initialized in the real-time loop
  HardwareReadWriteStatus read_write_status;
};
 
}  // namespace hardware_interface
#endif  // HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_