15 #ifndef HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
16 #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_
20 #include <unordered_map>
23 #include "hardware_interface/actuator.hpp"
24 #include "hardware_interface/hardware_component_info.hpp"
25 #include "hardware_interface/hardware_info.hpp"
26 #include "hardware_interface/loaned_command_interface.hpp"
27 #include "hardware_interface/loaned_state_interface.hpp"
28 #include "hardware_interface/sensor.hpp"
29 #include "hardware_interface/system.hpp"
30 #include "hardware_interface/system_interface.hpp"
31 #include "hardware_interface/types/hardware_interface_return_values.hpp"
32 #include "rclcpp/duration.hpp"
33 #include "rclcpp/node_interfaces/node_logging_interface.hpp"
34 #include "rclcpp/time.hpp"
38 class ResourceStorage;
39 class ControllerManager;
44 std::vector<std::string> failed_hardware_names;
52 rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
53 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface);
70 const std::string & urdf,
71 rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface,
72 rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface,
73 bool activate_all =
false,
const unsigned int update_rate = 100);
89 virtual bool load_and_initialize_components(
90 const std::string & urdf,
const unsigned int update_rate = 100);
100 bool are_components_initialized()
const;
118 std::vector<std::string> state_interface_keys()
const;
125 std::vector<std::string> available_state_interfaces()
const;
131 bool state_interface_is_available(
const std::string & name)
const;
143 void import_controller_exported_state_interfaces(
144 const std::string & controller_name, std::vector<StateInterface::ConstSharedPtr> & interfaces);
153 std::vector<std::string> get_controller_exported_state_interface_names(
154 const std::string & controller_name);
165 void make_controller_exported_state_interfaces_available(
const std::string & controller_name);
175 void make_controller_exported_state_interfaces_unavailable(
const std::string & controller_name);
184 void remove_controller_exported_state_interfaces(
const std::string & controller_name);
196 void import_controller_reference_interfaces(
197 const std::string & controller_name,
198 const std::vector<hardware_interface::CommandInterface::SharedPtr> & interfaces);
207 std::vector<std::string> get_controller_reference_interface_names(
208 const std::string & controller_name);
218 void make_controller_reference_interfaces_available(
const std::string & controller_name);
228 void make_controller_reference_interfaces_unavailable(
const std::string & controller_name);
237 void remove_controller_reference_interfaces(
const std::string & controller_name);
247 void cache_controller_to_hardware(
248 const std::string & controller_name,
const std::vector<std::string> & interfaces);
258 std::vector<std::string> get_cached_controllers_to_hardware(
const std::string & hardware_name);
269 bool command_interface_is_claimed(
const std::string & key)
const;
287 std::vector<std::string> command_interface_keys()
const;
294 std::vector<std::string> available_command_interfaces()
const;
301 bool command_interface_is_available(
const std::string & interface)
const;
307 size_t actuator_components_size()
const;
313 size_t sensor_components_size()
const;
319 size_t system_components_size()
const;
334 void import_component(
335 std::unique_ptr<ActuatorInterface> actuator,
const HardwareInfo & hardware_info);
350 void import_component(
351 std::unique_ptr<SensorInterface> sensor,
const HardwareInfo & hardware_info);
366 void import_component(
367 std::unique_ptr<SystemInterface> system,
const HardwareInfo & hardware_info);
373 std::unordered_map<std::string, HardwareComponentInfo> get_components_status();
392 bool prepare_command_mode_switch(
393 const std::vector<std::string> & start_interfaces,
394 const std::vector<std::string> & stop_interfaces);
409 bool perform_command_mode_switch(
410 const std::vector<std::string> & start_interfaces,
411 const std::vector<std::string> & stop_interfaces);
426 return_type set_component_state(
427 const std::string & component_name, rclcpp_lifecycle::State & target_state);
433 void shutdown_async_components();
458 bool command_interface_exists(
const std::string & key)
const;
464 bool state_interface_exists(
const std::string & key)
const;
471 rclcpp::Logger get_logger()
const;
477 rclcpp::Clock::SharedPtr get_clock()
const;
479 bool components_are_loaded_and_initialized_ =
false;
481 mutable std::recursive_mutex resource_interfaces_lock_;
482 mutable std::recursive_mutex claimed_command_interfaces_lock_;
483 mutable std::recursive_mutex resources_lock_;
486 bool validate_storage(
const std::vector<hardware_interface::HardwareInfo> & hardware_info)
const;
488 void release_command_interface(
const std::string & key);
490 std::unordered_map<std::string, bool> claimed_command_interface_map_;
492 std::unique_ptr<ResourceStorage> resource_storage_;
Definition: loaned_command_interface.hpp:30
Definition: loaned_state_interface.hpp:29
Definition: resource_manager.hpp:48
Definition: actuator.hpp:34
This structure stores information about hardware defined in a robot's URDF.
Definition: hardware_info.hpp:170
Definition: resource_manager.hpp:42