From f63890d98df8ed69a6ef1324826ed9e037c2291d Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 1 Aug 2025 10:47:52 +0000 Subject: [PATCH 01/17] added base class --- .../hardware_component_interface.hpp | 771 ++++++++++++++++++ 1 file changed, 771 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/hardware_component_interface.hpp diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp new file mode 100644 index 0000000000..cb96f50fc9 --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -0,0 +1,771 @@ +// 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 + +#include +#include +#include +#include +#include +#include + +#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_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" + +namespace hardware_interface +{ + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +/** + * @brief Virtual base class for all hardware components (Actuators, Sensors, and Systems). + * + * This class provides the common structure and functionality for all hardware components, + * including lifecycle management, interface handling, and asynchronous support. Hardware + * plugins should inherit from one of its derivatives: ActuatorInterface, SensorInterface, + * or SystemInterface. + */ +class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +{ +public: + HardwareComponentInterface() + : lifecycle_state_( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), + logger_(rclcpp::get_logger("hardware_component_interface")) + { + } + + /// HardwareComponentInterface copy constructor is actively deleted. + /** + * Hardware interfaces have unique ownership and thus can't be copied in order to avoid + * failed or simultaneous access to hardware. + */ + HardwareComponentInterface(const HardwareComponentInterface & other) = delete; + + HardwareComponentInterface(HardwareComponentInterface && other) = delete; + + virtual ~HardwareComponentInterface() = default; + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] hardware_info structure with data from URDF. + * \param[in] clock pointer to the resource manager clock. + * \param[in] logger Logger for the hardware component. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + [[deprecated( + "Replaced by CallbackReturn init(const hardware_interface::HardwareComponentParams & " + "params). Initialization is handled by the Framework.")]] + CallbackReturn init( + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + { + hardware_interface::HardwareComponentParams params; + params.hardware_info = hardware_info; + params.clock = clock; + params.logger = logger; + return init(params); + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF and also the + /// clock and logger interfaces. + /** + * \param[in] params A struct of type HardwareComponentParams containing all necessary + * parameters for initializing this specific hardware component, + * including its HardwareInfo, a dedicated logger, a clock, and a + * weak_ptr to the executor. + * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks + * such as `spin()`. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + CallbackReturn init(const hardware_interface::HardwareComponentParams & params) + { + clock_ = params.clock; + auto logger_copy = params.logger; + logger_ = logger_copy.get_child( + "hardware_component." + params.hardware_info.type + "." + params.hardware_info.name); + info_ = params.hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + const auto read_start_time = std::chrono::steady_clock::now(); + const auto ret_read = read(time, period); + const auto read_end_time = std::chrono::steady_clock::now(); + read_return_info_.store(ret_read, std::memory_order_release); + read_execution_time_.store( + std::chrono::duration_cast(read_end_time - read_start_time), + std::memory_order_release); + if (ret_read != return_type::OK) + { + return ret_read; + } + if (info_.type != "sensor") + { + const auto write_start_time = std::chrono::steady_clock::now(); + const auto ret_write = write(time, period); + const auto write_end_time = std::chrono::steady_clock::now(); + write_return_info_.store(ret_write, std::memory_order_release); + write_execution_time_.store( + std::chrono::duration_cast( + write_end_time - write_start_time), + std::memory_order_release); + return ret_write; + } + return return_type::OK; + }, + info_.thread_priority); + async_handler_->start_thread(); + } + + if (auto locked_executor = params.executor.lock()) + { + std::string node_name = params.hardware_info.name; + std::transform( + node_name.begin(), node_name.end(), node_name.begin(), + [](unsigned char c) { return std::tolower(c); }); + std::replace(node_name.begin(), node_name.end(), '/', '_'); + hardware_component_node_ = std::make_shared(node_name); + locked_executor->add_node(hardware_component_node_->get_node_base_interface()); + } + else + { + RCLCPP_WARN( + params.logger, + "Executor is not available during hardware component initialization for '%s'. Skipping " + "node creation!", + params.hardware_info.name.c_str()); + } + + hardware_interface::HardwareComponentInterfaceParams interface_params; + interface_params.hardware_info = info_; + interface_params.executor = params.executor; + return on_init(interface_params); + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] hardware_info structure with data from URDF. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + [[deprecated("Use on_init(const HardwareComponentInterfaceParams & params) instead.")]] + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) + { + info_ = hardware_info; + if (info_.type == "actuator") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + } + else if (info_.type == "sensor") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + } + else if (info_.type == "system") + { + parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); + parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); + parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); + parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); + parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); + } + return CallbackReturn::SUCCESS; + }; + + /// Initialization of the hardware interface from data parsed from the robot's URDF. + /** + * \param[in] params A struct of type hardware_interface::HardwareComponentInterfaceParams + * containing all necessary parameters for initializing this specific hardware component, + * specifically its HardwareInfo, and a weak_ptr to the executor. + * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks + * such as `spin()`. + * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. + * \returns CallbackReturn::ERROR if any error happens or data are missing. + */ + virtual CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) + { + // This is done for backward compatibility with the old on_init method. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return on_init(params.hardware_info); +#pragma GCC diagnostic pop + }; + + /// Exports all state interfaces for this hardware interface. + /** + * Old way of exporting the StateInterfaces. If a empty vector is returned then + * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned + * then the exporting of the StateInterfaces is only done with this function and the ownership is + * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not + * be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_state_interfaces() method. " + "Exporting is handled by the Framework.")]] + virtual std::vector export_state_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_state_interfaces() and only when empty vector is returned call + // on_export_state_interfaces() + return {}; + } + + /** + * Override this method to export custom StateInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_state_interfaces_ map. + * + * \return vector of descriptions to the unlisted StateInterfaces + */ + virtual std::vector + export_unlisted_state_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the StateInterfaces. The StateInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the interface. + * + * \return vector of shared pointers to the created and stored StateInterfaces + */ + virtual std::vector on_export_state_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_state_interface_descriptions(); + + std::vector state_interfaces; + state_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + + sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); + + // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_state_interfaces_.insert(std::make_pair(name, description)); + auto state_interface = std::make_shared(description); + hardware_states_.insert(std::make_pair(name, state_interface)); + unlisted_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + + for (const auto & [name, descr] : joint_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + joint_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : sensor_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + sensor_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + for (const auto & [name, descr] : gpio_state_interfaces_) + { + auto state_interface = std::make_shared(descr); + hardware_states_.insert(std::make_pair(name, state_interface)); + gpio_states_.push_back(state_interface); + state_interfaces.push_back(std::const_pointer_cast(state_interface)); + } + return state_interfaces; + } + + /// Exports all command interfaces for this hardware interface. + /** + * Old way of exporting the CommandInterfaces. If a empty vector is returned then + * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is + * returned then the exporting of the CommandInterfaces is only done with this function and the + * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., + * can then not be used. + * + * Note the ownership over the state interfaces is transferred to the caller. + * + * \return vector of state interfaces + */ + [[deprecated( + "Replaced by vector on_export_command_interfaces() method. " + "Exporting is handled by the Framework.")]] + virtual std::vector export_command_interfaces() + { + // return empty vector by default. For backward compatibility we try calling + // export_command_interfaces() and only when empty vector is returned call + // on_export_command_interfaces() + return {}; + } + + /** + * Override this method to export custom CommandInterfaces which are not defined in the URDF file. + * Those interfaces will be added to the unlisted_command_interfaces_ map. + * + * \return vector of descriptions to the unlisted CommandInterfaces + */ + virtual std::vector + export_unlisted_command_interface_descriptions() + { + // return empty vector by default. + return {}; + } + + /** + * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created + * according to the InterfaceDescription. The memory accessed by the controllers and hardware is + * assigned here and resides in the system_interface. + * + * Actuator and System components should override this method. Sensor components can use the + * default. + * + * \return vector of shared pointers to the created and stored CommandInterfaces + */ + virtual std::vector on_export_command_interfaces() + { + // import the unlisted interfaces + std::vector unlisted_interface_descriptions = + export_unlisted_command_interface_descriptions(); + + std::vector command_interfaces; + command_interfaces.reserve( + unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + + gpio_command_interfaces_.size()); + + // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to + // maps. + for (const auto & description : unlisted_interface_descriptions) + { + auto name = description.get_name(); + unlisted_command_interfaces_.insert(std::make_pair(name, description)); + auto command_interface = std::make_shared(description); + hardware_commands_.insert(std::make_pair(name, command_interface)); + unlisted_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : joint_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + hardware_commands_.insert(std::make_pair(name, command_interface)); + joint_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + + for (const auto & [name, descr] : gpio_command_interfaces_) + { + auto command_interface = std::make_shared(descr); + hardware_commands_.insert(std::make_pair(name, command_interface)); + gpio_commands_.push_back(command_interface); + command_interfaces.push_back(command_interface); + } + return command_interfaces; + } + + /// Prepare for a new command interface switch. + /** + * Prepare for any mode-switching required by the new command interface combination. + * + * \note This is a non-realtime evaluation of whether a set of command interface claims are + * possible, and call to start preparing data structures for the upcoming switch that will occur. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be prepared (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. + */ + virtual return_type prepare_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) + { + return return_type::OK; + } + + // Perform switching to the new command interface. + /** + * Perform the mode-switching for the new command interface combination. + * + * \note This is part of the realtime update loop, and should be fast. + * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. + * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. + * \return return_type::OK if the new command interface combination can be switched to (or) if the + * interface key is not relevant to this system. Returns return_type::ERROR otherwise. + */ + virtual return_type perform_command_mode_switch( + const std::vector & /*start_interfaces*/, + const std::vector & /*stop_interfaces*/) + { + return return_type::OK; + } + + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * The method is called in the resource_manager's read loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + HardwareComponentCycleStatus trigger_read( + const rclcpp::Time & time, const rclcpp::Duration & period) + { + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; + if (info_.is_async) + { + status.result = read_return_info_.load(std::memory_order_acquire); + const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); + if (read_exec_time.count() > 0) + { + status.execution_time = read_exec_time; + } + const auto result = async_handler_->trigger_async_callback(time, period); + status.successful = result.first; + if (!status.successful) + { + RCLCPP_WARN( + get_logger(), + "Trigger read/write called while the previous async trigger is still in progress for " + "hardware interface : '%s'. Failed to trigger read/write cycle!", + info_.name.c_str()); + status.result = return_type::OK; + return status; + } + } + else + { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = read(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + } + return status; + } + + /// Read the current state values from the hardware. + /** + * The data readings from the physical hardware has to be updated + * and reflected accordingly in the exported state interfaces. + * That is, the data pointed by the interfaces shall be updated. + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * The method is called in the resource_manager's write loop + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + HardwareComponentCycleStatus trigger_write( + const rclcpp::Time & time, const rclcpp::Duration & period) + { + HardwareComponentCycleStatus status; + status.result = return_type::ERROR; + if (info_.is_async) + { + status.successful = true; + const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); + if (write_exec_time.count() > 0) + { + status.execution_time = write_exec_time; + } + status.result = write_return_info_.load(std::memory_order_acquire); + } + else + { + const auto start_time = std::chrono::steady_clock::now(); + status.successful = true; + status.result = write(time, period); + status.execution_time = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time); + } + return status; + } + + /// Write the current command values to the hardware. + /** + * The physical hardware shall be updated with the latest value from + * the exported command interfaces. + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \return return_type::OK if the read was successful, return_type::ERROR otherwise. + */ + virtual return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + { + return return_type::OK; + } + + /// Get name of the hardware. + /** + * \return name. + */ + const std::string & get_name() const { return info_.name; } + + /// Get name of the hardware group to which it belongs to. + /** + * \return group name. + */ + const std::string & get_group_name() const { return info_.group; } + + /// Get life-cycle state of the hardware. + /** + * \return state. + */ + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } + + /// Set life-cycle state of the hardware. + /** + * \return state. + */ + void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) + { + lifecycle_state_ = new_state; + } + + template + void set_state(const std::string & interface_name, const T & value) + { + auto it = hardware_states_.find(interface_name); + if (it == hardware_states_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "State interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::unique_lock lock(handle->get_mutex()); + std::ignore = handle->set_value(lock, value); + } + + template + T get_state(const std::string & interface_name) const + { + auto it = hardware_states_.find(interface_name); + if (it == hardware_states_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "State interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::shared_lock lock(handle->get_mutex()); + const auto opt_value = handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to get state value from interface: {}. This should not happen."), + interface_name)); + } + return opt_value.value(); + } + + template + void set_command(const std::string & interface_name, const T & value) + { + auto it = hardware_commands_.find(interface_name); + if (it == hardware_commands_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "Command interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::unique_lock lock(handle->get_mutex()); + std::ignore = handle->set_value(lock, value); + } + + template + T get_command(const std::string & interface_name) const + { + auto it = hardware_commands_.find(interface_name); + if (it == hardware_commands_.end()) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE( + "Command interface not found: {} in hardware component: {}. " + "This should not happen."), + interface_name, info_.name)); + } + auto & handle = it->second; + std::shared_lock lock(handle->get_mutex()); + const auto opt_value = handle->get_optional(lock); + if (!opt_value) + { + throw std::runtime_error( + fmt::format( + FMT_COMPILE("Failed to get command value from interface: {}. This should not happen."), + interface_name)); + } + return opt_value.value(); + } + + /// Get the logger of the Interface. + /** + * \return logger of the Interface. + */ + rclcpp::Logger get_logger() const { return logger_; } + + /// Get the clock of the Interface. + /** + * \return clock of the Interface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_; } + + /// Get the default node of the Interface. + /** + * \return node of the Interface. + */ + rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } + + /// Get the hardware info of the Interface. + /** + * \return hardware info of the Interface. + */ + const HardwareInfo & get_hardware_info() const { return info_; } + + /// Prepare for the activation of the hardware. + /** + * This method is called before the hardware is activated by the resource manager. + */ + void prepare_for_activation() + { + read_return_info_.store(return_type::OK, std::memory_order_release); + read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + write_return_info_.store(return_type::OK, std::memory_order_release); + write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); + } + + /// Enable or disable introspection of the hardware. + /** + * \param[in] enable Enable introspection if true, disable otherwise. + */ + void enable_introspection(bool enable) + { + if (enable) + { + stats_registrations_.enableAll(); + } + else + { + stats_registrations_.disableAll(); + } + } + +protected: + HardwareInfo info_; + // interface names to InterfaceDescription + std::unordered_map joint_state_interfaces_; + std::unordered_map joint_command_interfaces_; + + std::unordered_map sensor_state_interfaces_; + + std::unordered_map gpio_state_interfaces_; + std::unordered_map gpio_command_interfaces_; + + std::unordered_map unlisted_state_interfaces_; + std::unordered_map unlisted_command_interfaces_; + + rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; + + // Exported Command- and StateInterfaces in order they are listed in the hardware description. + std::vector joint_states_; + std::vector joint_commands_; + + std::vector sensor_states_; + + std::vector gpio_states_; + std::vector gpio_commands_; + + std::vector unlisted_states_; + std::vector unlisted_commands_; + +private: + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_; + rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; + // interface names to Handle accessed through getters/setters + std::unordered_map hardware_states_; + std::unordered_map hardware_commands_; + std::atomic read_return_info_ = return_type::OK; + std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); + std::atomic write_return_info_ = return_type::OK; + std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); + +protected: + pal_statistics::RegistrationsRAII stats_registrations_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_INTERFACE_HPP_ From 0d3b8aa9b717fd47093ff2eea895b281f760a5ac Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 1 Aug 2025 10:51:57 +0000 Subject: [PATCH 02/17] added updated interface class definitions --- .../hardware_interface/actuator_interface.hpp | 679 +---------------- .../hardware_interface/sensor_interface.hpp | 441 +---------- .../hardware_interface/system_interface.hpp | 716 +----------------- 3 files changed, 12 insertions(+), 1824 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 875cb03942..995acd6db8 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -15,38 +15,10 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_INTERFACE_HPP_ -#include - -#include -#include -#include -#include -#include -#include - -#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/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/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/async_function_handler.hpp" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /// Virtual Class to implement when integrating a 1 DoF actuator into ros2_control. /** * The typical examples are conveyors or motors. @@ -79,7 +51,7 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * * ACTIVE (on_activate): * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. - * Command interfaces available. + * Command interfaces are available. * * \todo * Implement @@ -92,653 +64,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * HW_IF_EFFORT. */ -class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class ActuatorInterface : public HardwareComponentInterface { public: - ActuatorInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - actuator_logger_(rclcpp::get_logger("actuator_interface")) - { - } - - /// ActuatorInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - ActuatorInterface(const ActuatorInterface & other) = delete; - - ActuatorInterface(ActuatorInterface && other) = delete; - - virtual ~ActuatorInterface() = default; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated( - "Replaced by CallbackReturn init(const hardware_interface::HardwareComponentParams & " - "params). Initialization is handled by the Framework.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - { - hardware_interface::HardwareComponentParams params; - params.hardware_info = hardware_info; - params.clock = clock; - params.logger = logger; - return init(params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] params A struct of type HardwareComponentParams containing all necessary - * parameters for initializing this specific hardware component, - * including its HardwareInfo, a dedicated logger, a clock, and a - * weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init(const hardware_interface::HardwareComponentParams & params) - { - actuator_clock_ = params.clock; - auto logger_copy = params.logger; - actuator_logger_ = - logger_copy.get_child("hardware_component.actuator." + params.hardware_info.name); - info_ = params.hardware_info; - if (info_.is_async) - { - RCLCPP_INFO_STREAM( - get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); - async_handler_ = std::make_unique>(); - async_handler_->init( - [this](const rclcpp::Time & time, const rclcpp::Duration & period) - { - const auto read_start_time = std::chrono::steady_clock::now(); - const auto ret_read = read(time, period); - const auto read_end_time = std::chrono::steady_clock::now(); - read_return_info_.store(ret_read, std::memory_order_release); - read_execution_time_.store( - std::chrono::duration_cast(read_end_time - read_start_time), - std::memory_order_release); - if (ret_read != return_type::OK) - { - return ret_read; - } - const auto write_start_time = std::chrono::steady_clock::now(); - const auto ret_write = write(time, period); - const auto write_end_time = std::chrono::steady_clock::now(); - write_return_info_.store(ret_write, std::memory_order_release); - write_execution_time_.store( - std::chrono::duration_cast(write_end_time - write_start_time), - std::memory_order_release); - return ret_write; - }, - info_.thread_priority); - async_handler_->start_thread(); - } - - if (auto locked_executor = params.executor.lock()) - { - std::string node_name = params.hardware_info.name; - std::transform( - node_name.begin(), node_name.end(), node_name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(node_name.begin(), node_name.end(), '/', '_'); - hardware_component_node_ = std::make_shared(node_name); - locked_executor->add_node(hardware_component_node_->get_node_base_interface()); - } - else - { - RCLCPP_WARN( - params.logger, - "Executor is not available during hardware component initialization for '%s'. Skipping " - "node creation!", - params.hardware_info.name.c_str()); - } - - hardware_interface::HardwareComponentInterfaceParams interface_params; - interface_params.hardware_info = info_; - interface_params.executor = params.executor; - return on_init(interface_params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use on_init(const HardwareComponentInterfaceParams & params) instead.")]] - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) - { - info_ = hardware_info; - parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); - parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); - return CallbackReturn::SUCCESS; - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] params A struct of type hardware_interface::HardwareComponentInterfaceParams - * containing all necessary parameters for initializing this specific hardware component, - * specifically its HardwareInfo, and a weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - virtual CallbackReturn on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) - { - // This is done for backward compatibility with the old on_init method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return on_init(params.hardware_info); -#pragma GCC diagnostic pop - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] - virtual std::vector export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the actuator_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_state_interfaces_.size()); - - // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - actuator_states_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - actuator_states_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - return state_interfaces; - } - - /// Exports all command interfaces for this hardware interface. - /** - * Old way of exporting the CommandInterfaces. If a empty vector is returned then - * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is - * returned then the exporting of the CommandInterfaces is only done with this function and the - * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., - * can then not be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " - "Exporting is handled by the Framework.")]] - virtual std::vector export_command_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_command_interfaces() and only when empty vector is returned call - // on_export_command_interfaces() - return {}; - } - - /** - * Override this method to export custom CommandInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_command_interfaces_ map. - * - * \return vector of descriptions to the unlisted CommandInterfaces - */ - virtual std::vector - export_unlisted_command_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the actuator_interface. - * - * \return vector of shared pointers to the created and stored CommandInterfaces - */ - virtual std::vector on_export_command_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_command_interface_descriptions(); - - std::vector command_interfaces; - command_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_command_interfaces_.size()); - - // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_command_interfaces_.insert(std::make_pair(name, description)); - auto command_interface = std::make_shared(description); - actuator_commands_.insert(std::make_pair(name, command_interface)); - unlisted_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : joint_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - actuator_commands_.insert(std::make_pair(name, command_interface)); - joint_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - return command_interfaces; - } - /// Prepare for a new command interface switch. - /** - * Prepare for any mode-switching required by the new command interface combination. - * - * \note This is a non-realtime evaluation of whether a set of command interface claims are - * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return return_type::OK if the new command interface combination can be prepared (or) if the - * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise. - */ - virtual return_type prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - // Perform switching to the new command interface. - /** - * Perform the mode-switching for the new command interface combination. - * - * \note This is part of the realtime update loop, and should be fast. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return return_type::OK if the new command interface combination can be switched to (or) if the - * interface key is not relevant to this actuator. Returns return_type::ERROR otherwise. - */ - virtual return_type perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * The method is called in the resource_manager's read loop - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.result = read_return_info_.load(std::memory_order_acquire); - const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); - if (read_exec_time.count() > 0) - { - status.execution_time = read_exec_time; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - if (!status.successful) - { - RCLCPP_WARN( - get_logger(), - "Trigger read/write called while the previous async trigger is still in progress for " - "hardware interface : '%s'. Failed to trigger read/write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * The method is called in the resource_manager's write loop - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - HardwareComponentCycleStatus trigger_write( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.successful = true; - const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); - if (write_exec_time.count() > 0) - { - status.execution_time = write_exec_time; - } - status.result = write_return_info_.load(std::memory_order_acquire); - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = write(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Write the current command values to the actuator. - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = actuator_states_.find(interface_name); - if (it == actuator_states_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in actuator hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const - { - auto it = actuator_states_.find(interface_name); - if (it == actuator_states_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in actuator hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Failed to get state value from interface: {}. This should not happen."), - interface_name)); - } - return opt_value.value(); - } - - template - void set_command(const std::string & interface_name, const T & value) - { - auto it = actuator_commands_.find(interface_name); - if (it == actuator_commands_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Command interface not found: {} in actuator hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_command(const std::string & interface_name) const - { - auto it = actuator_commands_.find(interface_name); - if (it == actuator_commands_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Command interface not found: {} in actuator hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Failed to get command value from interface: {}. This should not happen."), - interface_name)); - } - return opt_value.value(); - } - - /// Get the logger of the ActuatorInterface. - /** - * \return logger of the ActuatorInterface. - */ - rclcpp::Logger get_logger() const { return actuator_logger_; } - - /// Get the clock of the ActuatorInterface. - /** - * \return clock of the ActuatorInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return actuator_clock_; } - - /// Get the default node of the ActuatorInterface. - /** - * \return node of the ActuatorInterface. - */ - rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } - - /// Get the hardware info of the ActuatorInterface. - /** - * \return hardware info of the ActuatorInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Prepare for the activation of the hardware. - /** - * This method is called before the hardware is activated by the resource manager. - */ - void prepare_for_activation() - { - read_return_info_.store(return_type::OK, std::memory_order_release); - read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - write_return_info_.store(return_type::OK, std::memory_order_release); - write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - } - - /// Enable or disable introspection of the hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -protected: - HardwareInfo info_; - // interface names to InterfaceDescription - std::unordered_map joint_state_interfaces_; - std::unordered_map joint_command_interfaces_; - - std::unordered_map unlisted_state_interfaces_; - std::unordered_map unlisted_command_interfaces_; - - // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector joint_states_; - std::vector joint_commands_; - - std::vector unlisted_states_; - std::vector unlisted_commands_; - - rclcpp_lifecycle::State lifecycle_state_; - std::unique_ptr> async_handler_; - -private: - rclcpp::Clock::SharedPtr actuator_clock_; - rclcpp::Logger actuator_logger_; - rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; - // interface names to Handle accessed through getters/setters - std::unordered_map actuator_states_; - std::unordered_map actuator_commands_; - std::atomic read_return_info_ = return_type::OK; - std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_ = return_type::OK; - std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override = 0; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 04522cad78..dffb700114 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -15,37 +15,12 @@ #ifndef HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SENSOR_INTERFACE_HPP_ -#include - -#include -#include -#include -#include -#include #include -#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/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/async_function_handler.hpp" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /// Virtual Class to implement when integrating a stand-alone sensor into ros2_control. /** * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). @@ -76,421 +51,15 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface * ACTIVE (on_activate): * States can be read. */ -class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class SensorInterface : public HardwareComponentInterface { public: - SensorInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - sensor_logger_(rclcpp::get_logger("sensor_interface")) - { - } - - /// SensorInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - SensorInterface(const SensorInterface & other) = delete; - - SensorInterface(SensorInterface && other) = delete; - - virtual ~SensorInterface() = default; + std::vector on_export_command_interfaces() override { return {}; } - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated( - "Replaced by CallbackReturn init(const hardware_interface::HardwareComponentParams & " - "params). Initialization is handled by the Framework.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) final { - hardware_interface::HardwareComponentParams params; - params.hardware_info = hardware_info; - params.clock = clock; - params.logger = logger; - return init(params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] params A struct of type HardwareComponentParams containing all necessary - * parameters for initializing this specific hardware component, - * including its HardwareInfo, a dedicated logger, a clock, and a - * weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init(const hardware_interface::HardwareComponentParams & params) - { - sensor_clock_ = params.clock; - auto logger_copy = params.logger; - sensor_logger_ = - logger_copy.get_child("hardware_component.sensor." + params.hardware_info.name); - info_ = params.hardware_info; - if (info_.is_async) - { - RCLCPP_INFO_STREAM( - get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); - read_async_handler_ = std::make_unique>(); - read_async_handler_->init( - std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2), - info_.thread_priority); - read_async_handler_->start_thread(); - } - - if (auto locked_executor = params.executor.lock()) - { - std::string node_name = params.hardware_info.name; - std::transform( - node_name.begin(), node_name.end(), node_name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(node_name.begin(), node_name.end(), '/', '_'); - hardware_component_node_ = std::make_shared(node_name); - locked_executor->add_node(hardware_component_node_->get_node_base_interface()); - } - else - { - RCLCPP_WARN( - params.logger, - "Executor is not available during hardware component initialization for '%s'. Skipping " - "node creation!", - params.hardware_info.name.c_str()); - } - - hardware_interface::HardwareComponentInterfaceParams interface_params; - interface_params.hardware_info = info_; - interface_params.executor = params.executor; - return on_init(interface_params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use on_init(const HardwareComponentInterfaceParams & params) instead.")]] - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) - { - info_ = hardware_info; - parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); - parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); - return CallbackReturn::SUCCESS; - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] params A struct of type hardware_interface::HardwareComponentInterfaceParams - * containing all necessary parameters for initializing this specific hardware component, - * specifically its HardwareInfo, and a weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - virtual CallbackReturn on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) - { - // This is done for backward compatibility with the old on_init method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return on_init(params.hardware_info); -#pragma GCC diagnostic pop - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] - virtual std::vector export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the sensor_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + sensor_state_interfaces_.size() + - joint_state_interfaces_.size()); - - // add InterfaceDescriptions and create StateInterfaces from the descriptions and add to maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : sensor_state_interfaces_) - { - // TODO(Manuel) check for duplicates otherwise only the first appearance of "name" is inserted - auto state_interface = std::make_shared(descr); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - sensor_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - sensor_states_map_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - return state_interfaces; + return return_type::OK; } - - /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - const auto result = read_async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - status.result = result.second; - const auto execution_time = read_async_handler_->get_last_execution_time(); - if (execution_time.count() > 0) - { - status.execution_time = execution_time; - } - if (!status.successful) - { - RCLCPP_WARN( - get_logger(), - "Trigger read called while read async handler is still in progress for hardware " - "interface : '%s'. Failed to trigger read cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = sensor_states_map_.find(interface_name); - if (it == sensor_states_map_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in sensor hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const - { - auto it = sensor_states_map_.find(interface_name); - if (it == sensor_states_map_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in sensor hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Failed to get state value from interface: {}. " - "This should not happen."), - interface_name)); - } - return opt_value.value(); - } - - /// Get the logger of the SensorInterface. - /** - * \return logger of the SensorInterface. - */ - rclcpp::Logger get_logger() const { return sensor_logger_; } - - /// Get the clock of the SensorInterface. - /** - * \return clock of the SensorInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return sensor_clock_; } - - /// Get the default node of the ActuatorInterface. - /** - * \return node of the ActuatorInterface. - */ - rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } - - /// Get the hardware info of the SensorInterface. - /** - * \return hardware info of the SensorInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Enable or disable introspection of the sensor hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -protected: - HardwareInfo info_; - // interface names to InterfaceDescription - std::unordered_map joint_state_interfaces_; - std::unordered_map sensor_state_interfaces_; - std::unordered_map unlisted_state_interfaces_; - - // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector joint_states_; - std::vector sensor_states_; - std::vector unlisted_states_; - - rclcpp_lifecycle::State lifecycle_state_; - std::unique_ptr> read_async_handler_; - -private: - rclcpp::Clock::SharedPtr sensor_clock_; - rclcpp::Logger sensor_logger_; - rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; - // interface names to Handle accessed through getters/setters - std::unordered_map sensor_states_map_; - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 07e1d5c724..39cf249fe4 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -15,40 +15,10 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ #define HARDWARE_INTERFACE__SYSTEM_INTERFACE_HPP_ -#include - -#include -#include -#include -#include -#include -#include - -#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_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" -#include "realtime_tools/async_function_handler.hpp" +#include "hardware_interface/hardware_component_interface.hpp" namespace hardware_interface { - -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - /** * @brief Virtual Class to implement when integrating a complex system into ros2_control. * @@ -96,690 +66,10 @@ using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface *HW_IF_EFFORT. */ -class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface +class SystemInterface : public HardwareComponentInterface { public: - SystemInterface() - : lifecycle_state_( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN, lifecycle_state_names::UNKNOWN)), - system_logger_(rclcpp::get_logger("system_interface")) - { - } - - /// SystemInterface copy constructor is actively deleted. - /** - * Hardware interfaces are having a unique ownership and thus can't be copied in order to avoid - * failed or simultaneous access to hardware. - */ - SystemInterface(const SystemInterface & other) = delete; - - SystemInterface(SystemInterface && other) = delete; - - virtual ~SystemInterface() = default; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] hardware_info structure with data from URDF. - * \param[in] clock pointer to the resource manager clock. - * \param[in] logger Logger for the hardware component. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated( - "Replaced by CallbackReturn init(const hardware_interface::HardwareComponentParams & " - "params). Initialization is handled by the Framework.")]] - CallbackReturn init( - const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) - { - hardware_interface::HardwareComponentParams params; - params.hardware_info = hardware_info; - params.clock = clock; - params.logger = logger; - return init(params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF and also the - /// clock and logger interfaces. - /** - * \param[in] params A struct of type HardwareComponentParams containing all necessary - * parameters for initializing this specific hardware component, - * including its HardwareInfo, a dedicated logger, a clock, and a - * weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - CallbackReturn init(const hardware_interface::HardwareComponentParams & params) - { - system_clock_ = params.clock; - auto logger_copy = params.logger; - system_logger_ = - logger_copy.get_child("hardware_component.system." + params.hardware_info.name); - info_ = params.hardware_info; - if (info_.is_async) - { - RCLCPP_INFO_STREAM( - get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); - async_handler_ = std::make_unique>(); - async_handler_->init( - [this](const rclcpp::Time & time, const rclcpp::Duration & period) - { - const auto read_start_time = std::chrono::steady_clock::now(); - const auto ret_read = read(time, period); - const auto read_end_time = std::chrono::steady_clock::now(); - read_return_info_.store(ret_read, std::memory_order_release); - read_execution_time_.store( - std::chrono::duration_cast(read_end_time - read_start_time), - std::memory_order_release); - if (ret_read != return_type::OK) - { - return ret_read; - } - const auto write_start_time = std::chrono::steady_clock::now(); - const auto ret_write = write(time, period); - const auto write_end_time = std::chrono::steady_clock::now(); - write_return_info_.store(ret_write, std::memory_order_release); - write_execution_time_.store( - std::chrono::duration_cast(write_end_time - write_start_time), - std::memory_order_release); - return ret_write; - }, - info_.thread_priority); - async_handler_->start_thread(); - } - - if (auto locked_executor = params.executor.lock()) - { - std::string node_name = params.hardware_info.name; - std::transform( - node_name.begin(), node_name.end(), node_name.begin(), - [](unsigned char c) { return std::tolower(c); }); - std::replace(node_name.begin(), node_name.end(), '/', '_'); - hardware_component_node_ = std::make_shared(node_name); - locked_executor->add_node(hardware_component_node_->get_node_base_interface()); - } - else - { - RCLCPP_WARN( - params.logger, - "Executor is not available during hardware component initialization for '%s'. Skipping " - "node creation!", - params.hardware_info.name.c_str()); - } - - hardware_interface::HardwareComponentInterfaceParams interface_params; - interface_params.hardware_info = info_; - interface_params.executor = params.executor; - return on_init(interface_params); - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] hardware_info structure with data from URDF. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - [[deprecated("Use on_init(const HardwareComponentInterfaceParams & params) instead.")]] - virtual CallbackReturn on_init(const HardwareInfo & hardware_info) - { - info_ = hardware_info; - parse_state_interface_descriptions(info_.joints, joint_state_interfaces_); - parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); - parse_state_interface_descriptions(info_.gpios, gpio_state_interfaces_); - parse_command_interface_descriptions(info_.joints, joint_command_interfaces_); - parse_command_interface_descriptions(info_.gpios, gpio_command_interfaces_); - return CallbackReturn::SUCCESS; - }; - - /// Initialization of the hardware interface from data parsed from the robot's URDF. - /** - * \param[in] params A struct of type hardware_interface::HardwareComponentInterfaceParams - * containing all necessary parameters for initializing this specific hardware component, - * specifically its HardwareInfo, and a weak_ptr to the executor. - * \warning The parsed executor should not be used to call `cancel()` or use blocking callbacks - * such as `spin()`. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ - virtual CallbackReturn on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) - { - // This is done for backward compatibility with the old on_init method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return on_init(params.hardware_info); -#pragma GCC diagnostic pop - }; - - /// Exports all state interfaces for this hardware interface. - /** - * Old way of exporting the StateInterfaces. If a empty vector is returned then - * the on_export_state_interfaces() method is called. If a vector with StateInterfaces is returned - * then the exporting of the StateInterfaces is only done with this function and the ownership is - * transferred to the resource manager. The set_command(...), get_command(...), ..., can then not - * be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_state_interfaces() method. " - "Exporting is handled by the Framework.")]] - virtual std::vector export_state_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_state_interfaces() and only when empty vector is returned call - // on_export_state_interfaces() - return {}; - } - - /** - * Override this method to export custom StateInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_state_interfaces_ map. - * - * \return vector of descriptions to the unlisted StateInterfaces - */ - virtual std::vector - export_unlisted_state_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the StateInterfaces. The StateInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * \return vector of shared pointers to the created and stored StateInterfaces - */ - virtual std::vector on_export_state_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_state_interface_descriptions(); - - std::vector state_interfaces; - state_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_state_interfaces_.size() + - sensor_state_interfaces_.size() + gpio_state_interfaces_.size()); - - // add InterfaceDescriptions and create the StateInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_state_interfaces_.insert(std::make_pair(name, description)); - auto state_interface = std::make_shared(description); - system_states_.insert(std::make_pair(name, state_interface)); - unlisted_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - - for (const auto & [name, descr] : joint_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - joint_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - for (const auto & [name, descr] : sensor_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - sensor_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - for (const auto & [name, descr] : gpio_state_interfaces_) - { - auto state_interface = std::make_shared(descr); - system_states_.insert(std::make_pair(name, state_interface)); - gpio_states_.push_back(state_interface); - state_interfaces.push_back(std::const_pointer_cast(state_interface)); - } - return state_interfaces; - } - - /// Exports all command interfaces for this hardware interface. - /** - * Old way of exporting the CommandInterfaces. If a empty vector is returned then - * the on_export_command_interfaces() method is called. If a vector with CommandInterfaces is - * returned then the exporting of the CommandInterfaces is only done with this function and the - * ownership is transferred to the resource manager. The set_command(...), get_command(...), ..., - * can then not be used. - * - * Note the ownership over the state interfaces is transferred to the caller. - * - * \return vector of state interfaces - */ - [[deprecated( - "Replaced by vector on_export_command_interfaces() method. " - "Exporting is handled by the Framework.")]] - virtual std::vector export_command_interfaces() - { - // return empty vector by default. For backward compatibility we try calling - // export_command_interfaces() and only when empty vector is returned call - // on_export_command_interfaces() - return {}; - } - - /** - * Override this method to export custom CommandInterfaces which are not defined in the URDF file. - * Those interfaces will be added to the unlisted_command_interfaces_ map. - * - * \return vector of descriptions to the unlisted CommandInterfaces - */ - virtual std::vector - export_unlisted_command_interface_descriptions() - { - // return empty vector by default. - return {}; - } - - /** - * Default implementation for exporting the CommandInterfaces. The CommandInterfaces are created - * according to the InterfaceDescription. The memory accessed by the controllers and hardware is - * assigned here and resides in the system_interface. - * - * \return vector of shared pointers to the created and stored CommandInterfaces - */ - virtual std::vector on_export_command_interfaces() - { - // import the unlisted interfaces - std::vector unlisted_interface_descriptions = - export_unlisted_command_interface_descriptions(); - - std::vector command_interfaces; - command_interfaces.reserve( - unlisted_interface_descriptions.size() + joint_command_interfaces_.size() + - gpio_command_interfaces_.size()); - - // add InterfaceDescriptions and create the CommandInterfaces from the descriptions and add to - // maps. - for (const auto & description : unlisted_interface_descriptions) - { - auto name = description.get_name(); - unlisted_command_interfaces_.insert(std::make_pair(name, description)); - auto command_interface = std::make_shared(description); - system_commands_.insert(std::make_pair(name, command_interface)); - unlisted_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : joint_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - system_commands_.insert(std::make_pair(name, command_interface)); - joint_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - - for (const auto & [name, descr] : gpio_command_interfaces_) - { - auto command_interface = std::make_shared(descr); - system_commands_.insert(std::make_pair(name, command_interface)); - gpio_commands_.push_back(command_interface); - command_interfaces.push_back(command_interface); - } - return command_interfaces; - } - - /// Prepare for a new command interface switch. - /** - * Prepare for any mode-switching required by the new command interface combination. - * - * \note This is a non-realtime evaluation of whether a set of command interface claims are - * possible, and call to start preparing data structures for the upcoming switch that will occur. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return return_type::OK if the new command interface combination can be prepared (or) if the - * interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type prepare_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - // Perform switching to the new command interface. - /** - * Perform the mode-switching for the new command interface combination. - * - * \note This is part of the realtime update loop, and should be fast. - * \param[in] start_interfaces vector of string identifiers for the command interfaces starting. - * \param[in] stop_interfaces vector of string identifiers for the command interfaces stopping. - * \return return_type::OK if the new command interface combination can be switched to (or) if the - * interface key is not relevant to this system. Returns return_type::ERROR otherwise. - */ - virtual return_type perform_command_mode_switch( - const std::vector & /*start_interfaces*/, - const std::vector & /*stop_interfaces*/) - { - return return_type::OK; - } - - /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * The method is called in the resource_manager's read loop - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - HardwareComponentCycleStatus trigger_read( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.result = read_return_info_.load(std::memory_order_acquire); - const auto read_exec_time = read_execution_time_.load(std::memory_order_acquire); - if (read_exec_time.count() > 0) - { - status.execution_time = read_exec_time; - } - const auto result = async_handler_->trigger_async_callback(time, period); - status.successful = result.first; - if (!status.successful) - { - RCLCPP_WARN( - get_logger(), - "Trigger read/write called while the previous async trigger is still in progress for " - "hardware interface : '%s'. Failed to trigger read/write cycle!", - info_.name.c_str()); - status.result = return_type::OK; - return status; - } - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = read(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Read the current state values from the actuator. - /** - * The data readings from the physical hardware has to be updated - * and reflected accordingly in the exported state interfaces. - * That is, the data pointed by the interfaces shall be updated. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - virtual return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * The method is called in the resource_manager's write loop - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - HardwareComponentCycleStatus trigger_write( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - HardwareComponentCycleStatus status; - status.result = return_type::ERROR; - if (info_.is_async) - { - status.successful = true; - const auto write_exec_time = write_execution_time_.load(std::memory_order_acquire); - if (write_exec_time.count() > 0) - { - status.execution_time = write_exec_time; - } - status.result = write_return_info_.load(std::memory_order_acquire); - } - else - { - const auto start_time = std::chrono::steady_clock::now(); - status.successful = true; - status.result = write(time, period); - status.execution_time = std::chrono::duration_cast( - std::chrono::steady_clock::now() - start_time); - } - return status; - } - - /// Write the current command values to the actuator. - /** - * The physical hardware shall be updated with the latest value from - * the exported command interfaces. - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured time taken by the last control loop iteration - * \return return_type::OK if the read was successful, return_type::ERROR otherwise. - */ - virtual return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - - /// Get name of the actuator hardware. - /** - * \return name. - */ - const std::string & get_name() const { return info_.name; } - - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ - const std::string & get_group_name() const { return info_.group; } - - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ - const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } - - /// Set life-cycle state of the actuator hardware. - /** - * \return state. - */ - void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) - { - lifecycle_state_ = new_state; - } - - template - void set_state(const std::string & interface_name, const T & value) - { - auto it = system_states_.find(interface_name); - if (it == system_states_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in system hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_state(const std::string & interface_name) const - { - auto it = system_states_.find(interface_name); - if (it == system_states_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "State interface not found: {} in system hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Failed to get state value from interface: {}. This should not happen."), - interface_name)); - } - return opt_value.value(); - } - - template - void set_command(const std::string & interface_name, const T & value) - { - auto it = system_commands_.find(interface_name); - if (it == system_commands_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Command interface not found: {} in system hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::unique_lock lock(handle->get_mutex()); - std::ignore = handle->set_value(lock, value); - } - - template - T get_command(const std::string & interface_name) const - { - auto it = system_commands_.find(interface_name); - if (it == system_commands_.end()) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE( - "Command interface not found: {} in system hardware component: {}. " - "This should not happen."), - interface_name, info_.name)); - } - auto & handle = it->second; - std::shared_lock lock(handle->get_mutex()); - const auto opt_value = handle->get_optional(lock); - if (!opt_value) - { - throw std::runtime_error( - fmt::format( - FMT_COMPILE("Failed to get command value from interface: {}. This should not happen."), - interface_name)); - } - return opt_value.value(); - } - - /// Get the logger of the SystemInterface. - /** - * \return logger of the SystemInterface. - */ - rclcpp::Logger get_logger() const { return system_logger_; } - - /// Get the clock of the SystemInterface. - /** - * \return clock of the SystemInterface. - */ - rclcpp::Clock::SharedPtr get_clock() const { return system_clock_; } - - /// Get the default node of the ActuatorInterface. - /** - * \return node of the ActuatorInterface. - */ - rclcpp::Node::SharedPtr get_node() const { return hardware_component_node_; } - - /// Get the hardware info of the SystemInterface. - /** - * \return hardware info of the SystemInterface. - */ - const HardwareInfo & get_hardware_info() const { return info_; } - - /// Prepare for the activation of the hardware. - /** - * This method is called before the hardware is activated by the resource manager. - */ - void prepare_for_activation() - { - read_return_info_.store(return_type::OK, std::memory_order_release); - read_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - write_return_info_.store(return_type::OK, std::memory_order_release); - write_execution_time_.store(std::chrono::nanoseconds::zero(), std::memory_order_release); - } - - /// Enable or disable introspection of the hardware. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ - void enable_introspection(bool enable) - { - if (enable) - { - stats_registrations_.enableAll(); - } - else - { - stats_registrations_.disableAll(); - } - } - -protected: - HardwareInfo info_; - // interface names to InterfaceDescription - std::unordered_map joint_state_interfaces_; - std::unordered_map joint_command_interfaces_; - - std::unordered_map sensor_state_interfaces_; - - std::unordered_map gpio_state_interfaces_; - std::unordered_map gpio_command_interfaces_; - - std::unordered_map unlisted_state_interfaces_; - std::unordered_map unlisted_command_interfaces_; - - rclcpp_lifecycle::State lifecycle_state_; - std::unique_ptr> async_handler_; - - // Exported Command- and StateInterfaces in order they are listed in the hardware description. - std::vector joint_states_; - std::vector joint_commands_; - - std::vector sensor_states_; - - std::vector gpio_states_; - std::vector gpio_commands_; - - std::vector unlisted_states_; - std::vector unlisted_commands_; - -private: - rclcpp::Clock::SharedPtr system_clock_; - rclcpp::Logger system_logger_; - rclcpp::Node::SharedPtr hardware_component_node_ = nullptr; - // interface names to Handle accessed through getters/setters - std::unordered_map system_states_; - std::unordered_map system_commands_; - std::atomic read_return_info_ = return_type::OK; - std::atomic read_execution_time_ = std::chrono::nanoseconds::zero(); - std::atomic write_return_info_ = return_type::OK; - std::atomic write_execution_time_ = std::chrono::nanoseconds::zero(); - -protected: - pal_statistics::RegistrationsRAII stats_registrations_; + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override = 0; }; } // namespace hardware_interface From 67960dbf6f9d196e3a224ab9fc5aef4aef942807 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 09:36:22 +0000 Subject: [PATCH 03/17] added base class for hardware component --- .../hardware_interface/hardware_component.hpp | 127 ++++++ hardware_interface/src/hardware_component.cpp | 431 ++++++++++++++++++ 2 files changed, 558 insertions(+) create mode 100644 hardware_interface/include/hardware_interface/hardware_component.hpp create mode 100644 hardware_interface/src/hardware_component.cpp diff --git a/hardware_interface/include/hardware_interface/hardware_component.hpp b/hardware_interface/include/hardware_interface/hardware_component.hpp new file mode 100644 index 0000000000..2d506f8bd1 --- /dev/null +++ b/hardware_interface/include/hardware_interface/hardware_component.hpp @@ -0,0 +1,127 @@ +// Copyright 2020 - 2021 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_HPP_ +#define HARDWARE_INTERFACE__HARDWARE_COMPONENT_HPP_ + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_component_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/statistics_types.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/node_interfaces/node_clock_interface.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace hardware_interface +{ +class HardwareComponentInterface; + +class HardwareComponent final +{ +public: + HardwareComponent() = default; + + explicit HardwareComponent(std::unique_ptr impl); + + explicit HardwareComponent(HardwareComponent && other) noexcept; + + ~HardwareComponent() = default; + + HardwareComponent(const HardwareComponent & other) = delete; + + HardwareComponent & operator=(const HardwareComponent & other) = delete; + + HardwareComponent & operator=(HardwareComponent && other) = delete; + + [[deprecated( + "Replaced by const rclcpp_lifecycle::State & initialize(const " + "hardware_interface::HardwareComponentParams & params).")]] + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & component_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); + + [[deprecated( + "Replaced by const rclcpp_lifecycle::State & initialize(const " + "hardware_interface::HardwareComponentParams & params).")]] + const rclcpp_lifecycle::State & initialize( + const HardwareInfo & component_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); + + const rclcpp_lifecycle::State & initialize( + const hardware_interface::HardwareComponentParams & params); + + const rclcpp_lifecycle::State & configure(); + + const rclcpp_lifecycle::State & cleanup(); + + const rclcpp_lifecycle::State & shutdown(); + + const rclcpp_lifecycle::State & activate(); + + const rclcpp_lifecycle::State & deactivate(); + + const rclcpp_lifecycle::State & error(); + + std::vector export_state_interfaces(); + + std::vector export_command_interfaces(); + + return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces); + + return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces); + + const std::string & get_name() const; + + const std::string & get_group_name() const; + + const rclcpp_lifecycle::State & get_lifecycle_state() const; + + const rclcpp::Time & get_last_read_time() const; + + const rclcpp::Time & get_last_write_time() const; + + const HardwareComponentStatisticsCollector & get_read_statistics() const; + + const HardwareComponentStatisticsCollector & get_write_statistics() const; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); + + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); + + std::recursive_mutex & get_mutex(); + +private: + std::unique_ptr impl_; + mutable std::recursive_mutex component_mutex_; + // Last read cycle time + rclcpp::Time last_read_cycle_time_; + // Last write cycle time + rclcpp::Time last_write_cycle_time_; + // Component statistics + HardwareComponentStatisticsCollector read_statistics_; + HardwareComponentStatisticsCollector write_statistics_; +}; + +} // namespace hardware_interface +#endif // HARDWARE_INTERFACE__HARDWARE_COMPONENT_HPP_ diff --git a/hardware_interface/src/hardware_component.cpp b/hardware_interface/src/hardware_component.cpp new file mode 100644 index 0000000000..7e4651fea0 --- /dev/null +++ b/hardware_interface/src/hardware_component.cpp @@ -0,0 +1,431 @@ +// 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. + +#include "hardware_interface/hardware_component.hpp" + +#include +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_component_interface.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/lifecycle_helpers.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace hardware_interface +{ +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + +HardwareComponent::HardwareComponent(std::unique_ptr impl) +: impl_(std::move(impl)) +{ +} + +HardwareComponent::HardwareComponent(HardwareComponent && other) noexcept +{ + std::lock_guard lock(other.component_mutex_); + impl_ = std::move(other.impl_); + last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); +} + +const rclcpp_lifecycle::State & HardwareComponent::initialize( + const HardwareInfo & hardware_info, rclcpp::Logger logger, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +{ + // This is done for backward compatibility with the old initialize method. +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + return this->initialize(hardware_info, logger, clock_interface->get_clock()); +#pragma GCC diagnostic pop +} + +const rclcpp_lifecycle::State & HardwareComponent::initialize( + const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) +{ + hardware_interface::HardwareComponentParams params; + params.hardware_info = hardware_info; + params.logger = logger; + params.clock = clock; + return initialize(params); +} + +const rclcpp_lifecycle::State & HardwareComponent::initialize( + const hardware_interface::HardwareComponentParams & params) +{ + std::unique_lock lock(component_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) + { + switch (impl_->init(params)) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::configure() +{ + std::unique_lock lock(component_mutex_); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_configure(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::cleanup() +{ + std::unique_lock lock(component_mutex_); + impl_->enable_introspection(false); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + switch (impl_->on_cleanup(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::shutdown() +{ + std::unique_lock lock(component_mutex_); + impl_->enable_introspection(false); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) + { + switch (impl_->on_shutdown(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::activate() +{ + std::unique_lock lock(component_mutex_); + last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); + read_statistics_.reset_statistics(); + if (impl_->get_hardware_info().type != "sensor") + { + write_statistics_.reset_statistics(); + } + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) + { + impl_->prepare_for_activation(); + switch (impl_->on_activate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->enable_introspection(true); + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::deactivate() +{ + std::unique_lock lock(component_mutex_); + impl_->enable_introspection(false); + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + switch (impl_->on_deactivate(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); + break; + case CallbackReturn::FAILURE: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); + break; + case CallbackReturn::ERROR: + impl_->set_lifecycle_state(error()); + break; + } + } + return impl_->get_lifecycle_state(); +} + +const rclcpp_lifecycle::State & HardwareComponent::error() +{ + std::unique_lock lock(component_mutex_); + impl_->enable_introspection(false); + if ( + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && + impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) + { + switch (impl_->on_error(impl_->get_lifecycle_state())) + { + case CallbackReturn::SUCCESS: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + lifecycle_state_names::UNCONFIGURED)); + break; + case CallbackReturn::FAILURE: + case CallbackReturn::ERROR: + impl_->set_lifecycle_state( + rclcpp_lifecycle::State( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); + break; + } + } + return impl_->get_lifecycle_state(); +} + +std::vector HardwareComponent::export_state_interfaces() +{ +// BEGIN (Handle export change): for backward compatibility, can be removed if +// export_command_interfaces() method is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + std::vector interfaces = impl_->export_state_interfaces(); +#pragma GCC diagnostic pop + // END: for backward compatibility + + // If no StateInterfaces has been exported, this could mean: + // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well + // b) default implementation for export_state_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_state_interfaces(); + } + + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto const & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(interface)); + } + return interface_ptrs; + // END: for backward compatibility +} + +std::vector HardwareComponent::export_command_interfaces() +{ +// BEGIN (Handle export change): for backward compatibility, can be removed if +// export_command_interfaces() method is removed +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wdeprecated-declarations" + std::vector interfaces = impl_->export_command_interfaces(); +#pragma GCC diagnostic pop + // END: for backward compatibility + + // If no CommandInterface has been exported, this could mean: + // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well + // b) default implementation for export_command_interfaces() is used -> new functionality -> + // Framework exports and creates everything + if (interfaces.empty()) + { + return impl_->on_export_command_interfaces(); + } + // BEGIN (Handle export change): for backward compatibility, can be removed if + // export_command_interfaces() method is removed + std::vector interface_ptrs; + interface_ptrs.reserve(interfaces.size()); + for (auto & interface : interfaces) + { + interface_ptrs.emplace_back(std::make_shared(std::move(interface))); + } + return interface_ptrs; + // END: for backward compatibility +} + +return_type HardwareComponent::prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + return impl_->prepare_command_mode_switch(start_interfaces, stop_interfaces); +} + +return_type HardwareComponent::perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) +{ + return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); +} + +const std::string & HardwareComponent::get_name() const { return impl_->get_name(); } + +const std::string & HardwareComponent::get_group_name() const { return impl_->get_group_name(); } + +const rclcpp_lifecycle::State & HardwareComponent::get_lifecycle_state() const +{ + return impl_->get_lifecycle_state(); +} + +const rclcpp::Time & HardwareComponent::get_last_read_time() const { return last_read_cycle_time_; } + +const rclcpp::Time & HardwareComponent::get_last_write_time() const +{ + return last_write_cycle_time_; +} + +const HardwareComponentStatisticsCollector & HardwareComponent::get_read_statistics() const +{ + return read_statistics_; +} + +const HardwareComponentStatisticsCollector & HardwareComponent::get_write_statistics() const +{ + return write_statistics_; +} + +return_type HardwareComponent::read(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_read_cycle_time_ = time; + return return_type::OK; + } + if ( + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + const auto trigger_result = impl_->trigger_read(time, period); + if (trigger_result.result == return_type::ERROR) + { + error(); + } + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + read_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + read_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_read_cycle_time_).seconds()); + } + last_read_cycle_time_ = time; + } + return trigger_result.result; + } + return return_type::OK; +} + +return_type HardwareComponent::write(const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (impl_->get_hardware_info().type == "sensor") + { + return return_type::OK; + } + + if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) + { + last_write_cycle_time_ = time; + return return_type::OK; + } + // only call write in the active state + if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + const auto trigger_result = impl_->trigger_write(time, period); + if (trigger_result.result == return_type::ERROR) + { + error(); + } + if (trigger_result.successful) + { + if (trigger_result.execution_time.has_value()) + { + write_statistics_.execution_time->AddMeasurement( + static_cast(trigger_result.execution_time.value().count()) / 1.e3); + } + if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) + { + write_statistics_.periodicity->AddMeasurement( + 1.0 / (time - last_write_cycle_time_).seconds()); + } + last_write_cycle_time_ = time; + } + return trigger_result.result; + } + return return_type::OK; +} + +std::recursive_mutex & HardwareComponent::get_mutex() { return component_mutex_; } +} // namespace hardware_interface From 1169f7b5dc1b5804f8d9d3049cd9daf5726ef7a0 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 09:36:55 +0000 Subject: [PATCH 04/17] added alias based sensor, system, actuator --- .../include/hardware_interface/actuator.hpp | 106 +----------------- .../include/hardware_interface/sensor.hpp | 89 +-------------- .../include/hardware_interface/system.hpp | 106 +----------------- 3 files changed, 6 insertions(+), 295 deletions(-) diff --git a/hardware_interface/include/hardware_interface/actuator.hpp b/hardware_interface/include/hardware_interface/actuator.hpp index b98b06b0dc..a9d696a176 100644 --- a/hardware_interface/include/hardware_interface/actuator.hpp +++ b/hardware_interface/include/hardware_interface/actuator.hpp @@ -15,113 +15,11 @@ #ifndef HARDWARE_INTERFACE__ACTUATOR_HPP_ #define HARDWARE_INTERFACE__ACTUATOR_HPP_ -#include -#include -#include - #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" +#include "hardware_interface/hardware_component.hpp" namespace hardware_interface { -class ActuatorInterface; - -class Actuator final -{ -public: - Actuator() = default; - - explicit Actuator(std::unique_ptr impl); - - explicit Actuator(Actuator && other) noexcept; - - ~Actuator() = default; - - Actuator(const Actuator & other) = delete; - - Actuator & operator=(const Actuator & other) = delete; - - Actuator & operator=(Actuator && other) = delete; - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & initialize( - const hardware_interface::HardwareComponentParams & params); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - std::vector export_command_interfaces(); - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const rclcpp::Time & get_last_write_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - const HardwareComponentStatisticsCollector & get_write_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); - - std::recursive_mutex & get_mutex(); - -private: - std::unique_ptr impl_; - mutable std::recursive_mutex actuators_mutex_; - // Last read cycle time - rclcpp::Time last_read_cycle_time_; - // Last write cycle time - rclcpp::Time last_write_cycle_time_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; - HardwareComponentStatisticsCollector write_statistics_; -}; - +using Actuator = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__ACTUATOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp index e6877a1b66..e3e7310bd5 100644 --- a/hardware_interface/include/hardware_interface/sensor.hpp +++ b/hardware_interface/include/hardware_interface/sensor.hpp @@ -15,96 +15,11 @@ #ifndef HARDWARE_INTERFACE__SENSOR_HPP_ #define HARDWARE_INTERFACE__SENSOR_HPP_ -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_component.hpp" #include "hardware_interface/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { -class SensorInterface; - -class Sensor final -{ -public: - Sensor() = default; - - explicit Sensor(std::unique_ptr impl); - - explicit Sensor(Sensor && other) noexcept; - - ~Sensor() = default; - - Sensor(const Sensor & other) = delete; - - Sensor & operator=(const Sensor & other) = delete; - - Sensor & operator=(Sensor && other) = delete; - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & initialize( - const hardware_interface::HardwareComponentParams & params); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } - - std::recursive_mutex & get_mutex(); - -private: - std::unique_ptr impl_; - mutable std::recursive_mutex sensors_mutex_; - // Last read cycle time - rclcpp::Time last_read_cycle_time_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; -}; - +using Sensor = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__SENSOR_HPP_ diff --git a/hardware_interface/include/hardware_interface/system.hpp b/hardware_interface/include/hardware_interface/system.hpp index 6b854fae86..4543020cfd 100644 --- a/hardware_interface/include/hardware_interface/system.hpp +++ b/hardware_interface/include/hardware_interface/system.hpp @@ -15,113 +15,11 @@ #ifndef HARDWARE_INTERFACE__SYSTEM_HPP_ #define HARDWARE_INTERFACE__SYSTEM_HPP_ -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/hardware_component.hpp" #include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/statistics_types.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/logger.hpp" -#include "rclcpp/node_interfaces/node_clock_interface.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" namespace hardware_interface { -class SystemInterface; - -class System final -{ -public: - System() = default; - - explicit System(std::unique_ptr impl); - - explicit System(System && other) noexcept; - - ~System() = default; - - System(const System & other) = delete; - - System & operator=(const System & other) = delete; - - System & operator=(System && other) = delete; - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface); - - [[deprecated( - "Replaced by const rclcpp_lifecycle::State & initialize(const " - "hardware_interface::HardwareComponentParams & params).")]] - const rclcpp_lifecycle::State & initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock); - - const rclcpp_lifecycle::State & initialize( - const hardware_interface::HardwareComponentParams & params); - - const rclcpp_lifecycle::State & configure(); - - const rclcpp_lifecycle::State & cleanup(); - - const rclcpp_lifecycle::State & shutdown(); - - const rclcpp_lifecycle::State & activate(); - - const rclcpp_lifecycle::State & deactivate(); - - const rclcpp_lifecycle::State & error(); - - std::vector export_state_interfaces(); - - std::vector export_command_interfaces(); - - return_type prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - return_type perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces); - - const std::string & get_name() const; - - const std::string & get_group_name() const; - - const rclcpp_lifecycle::State & get_lifecycle_state() const; - - const rclcpp::Time & get_last_read_time() const; - - const rclcpp::Time & get_last_write_time() const; - - const HardwareComponentStatisticsCollector & get_read_statistics() const; - - const HardwareComponentStatisticsCollector & get_write_statistics() const; - - return_type read(const rclcpp::Time & time, const rclcpp::Duration & period); - - return_type write(const rclcpp::Time & time, const rclcpp::Duration & period); - - std::recursive_mutex & get_mutex(); - -private: - std::unique_ptr impl_; - mutable std::recursive_mutex system_mutex_; - // Last read cycle time - rclcpp::Time last_read_cycle_time_; - // Last write cycle time - rclcpp::Time last_write_cycle_time_; - // Component statistics - HardwareComponentStatisticsCollector read_statistics_; - HardwareComponentStatisticsCollector write_statistics_; -}; - +using System = HardwareComponent; } // namespace hardware_interface #endif // HARDWARE_INTERFACE__SYSTEM_HPP_ From 8e2836b7d8332caca01318c56704147e03a06536 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 09:37:09 +0000 Subject: [PATCH 05/17] removed redundant .cpp files --- hardware_interface/CMakeLists.txt | 7 +- hardware_interface/src/actuator.cpp | 420 ---------------------------- hardware_interface/src/sensor.cpp | 329 ---------------------- hardware_interface/src/system.cpp | 417 --------------------------- 4 files changed, 4 insertions(+), 1169 deletions(-) delete mode 100644 hardware_interface/src/actuator.cpp delete mode 100644 hardware_interface/src/sensor.cpp delete mode 100644 hardware_interface/src/system.cpp diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f02b12901b..3b216d4108 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,11 +29,12 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(hardware_interface SHARED - src/actuator.cpp + # src/actuator.cpp src/component_parser.cpp src/resource_manager.cpp - src/sensor.cpp - src/system.cpp + src/hardware_component.cpp + # src/sensor.cpp + # src/system.cpp src/lexical_casts.cpp ) target_compile_features(hardware_interface PUBLIC cxx_std_17) diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp deleted file mode 100644 index 9aa9cc2e76..0000000000 --- a/hardware_interface/src/actuator.cpp +++ /dev/null @@ -1,420 +0,0 @@ -// Copyright 2020-2021 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. - -#include "hardware_interface/actuator.hpp" - -#include -#include -#include -#include - -#include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/lifecycle_helpers.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -// TODO(destogl): Add transition names also -namespace hardware_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -Actuator::Actuator(std::unique_ptr impl) : impl_(std::move(impl)) {} - -Actuator::Actuator(Actuator && other) noexcept -{ - std::lock_guard lock(other.actuators_mutex_); - impl_ = std::move(other.impl_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); -} - -const rclcpp_lifecycle::State & Actuator::initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - // This is done for backward compatibility with the old initialize method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return this->initialize(actuator_info, logger, clock_interface->get_clock()); -#pragma GCC diagnostic pop -} - -const rclcpp_lifecycle::State & Actuator::initialize( - const HardwareInfo & actuator_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -{ - hardware_interface::HardwareComponentParams params; - params.hardware_info = actuator_info; - params.logger = logger; - params.clock = clock; - return initialize(params); -} - -const rclcpp_lifecycle::State & Actuator::initialize( - const hardware_interface::HardwareComponentParams & params) -{ - std::unique_lock lock(actuators_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) - { - switch (impl_->init(params)) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::configure() -{ - std::unique_lock lock(actuators_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_configure(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::cleanup() -{ - std::unique_lock lock(actuators_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_cleanup(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::shutdown() -{ - std::unique_lock lock(actuators_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - switch (impl_->on_shutdown(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::activate() -{ - std::unique_lock lock(actuators_mutex_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - read_statistics_.reset_statistics(); - write_statistics_.reset_statistics(); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - impl_->prepare_for_activation(); - switch (impl_->on_activate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->enable_introspection(true); - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::deactivate() -{ - std::unique_lock lock(actuators_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - switch (impl_->on_deactivate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Actuator::error() -{ - std::unique_lock lock(actuators_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_error(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -std::vector Actuator::export_state_interfaces() -{ -// BEGIN (Handle export change): for backward compatibility, can be removed if -// export_command_interfaces() method is removed -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector interfaces = impl_->export_state_interfaces(); -#pragma GCC diagnostic pop - // END: for backward compatibility - - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_state_interfaces(); - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto const & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(interface)); - } - return interface_ptrs; - // END: for backward compatibility -} - -std::vector Actuator::export_command_interfaces() -{ -// BEGIN (Handle export change): for backward compatibility, can be removed if -// export_command_interfaces() method is removed -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector interfaces = impl_->export_command_interfaces(); -#pragma GCC diagnostic pop - // END: for backward compatibility - - // If no CommandInterface has been exported, this could mean: - // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well - // b) default implementation for export_command_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_command_interfaces(); - } - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(std::move(interface))); - } - return interface_ptrs; - // END: for backward compatibility -} - -return_type Actuator::prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->prepare_command_mode_switch(start_interfaces, stop_interfaces); -} - -return_type Actuator::perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); -} - -const std::string & Actuator::get_name() const { return impl_->get_name(); } - -const std::string & Actuator::get_group_name() const { return impl_->get_group_name(); } - -const rclcpp_lifecycle::State & Actuator::get_lifecycle_state() const -{ - return impl_->get_lifecycle_state(); -} - -const rclcpp::Time & Actuator::get_last_read_time() const { return last_read_cycle_time_; } - -const rclcpp::Time & Actuator::get_last_write_time() const { return last_write_cycle_time_; } - -const HardwareComponentStatisticsCollector & Actuator::get_read_statistics() const -{ - return read_statistics_; -} - -const HardwareComponentStatisticsCollector & Actuator::get_write_statistics() const -{ - return write_statistics_; -} - -return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_read_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_read(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - read_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_read_cycle_time_).seconds()); - } - last_read_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_write_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_write(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - write_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - write_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_write_cycle_time_).seconds()); - } - last_write_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -std::recursive_mutex & Actuator::get_mutex() { return actuators_mutex_; } -} // namespace hardware_interface diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp deleted file mode 100644 index ad0655d0df..0000000000 --- a/hardware_interface/src/sensor.cpp +++ /dev/null @@ -1,329 +0,0 @@ -// Copyright 2020 - 2021 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. - -#include "hardware_interface/sensor.hpp" - -#include -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/lifecycle_helpers.hpp" -#include "hardware_interface/sensor_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/logging.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace hardware_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -Sensor::Sensor(std::unique_ptr impl) : impl_(std::move(impl)) {} - -Sensor::Sensor(Sensor && other) noexcept -{ - std::lock_guard lock(other.sensors_mutex_); - impl_ = std::move(other.impl_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); -} - -const rclcpp_lifecycle::State & Sensor::initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - // This is done for backward compatibility with the old initialize method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return this->initialize(sensor_info, logger, clock_interface->get_clock()); -#pragma GCC diagnostic pop -} - -const rclcpp_lifecycle::State & Sensor::initialize( - const HardwareInfo & sensor_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -{ - hardware_interface::HardwareComponentParams params; - params.hardware_info = sensor_info; - params.logger = logger; - params.clock = clock; - return initialize(params); -} - -const rclcpp_lifecycle::State & Sensor::initialize( - const hardware_interface::HardwareComponentParams & params) -{ - std::unique_lock lock(sensors_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) - { - switch (impl_->init(params)) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::configure() -{ - std::unique_lock lock(sensors_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_configure(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::cleanup() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_cleanup(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::shutdown() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - switch (impl_->on_shutdown(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::activate() -{ - std::unique_lock lock(sensors_mutex_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - read_statistics_.reset_statistics(); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_activate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->enable_introspection(true); - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::deactivate() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - switch (impl_->on_deactivate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & Sensor::error() -{ - std::unique_lock lock(sensors_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_error(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -std::vector Sensor::export_state_interfaces() -{ -// BEGIN (Handle export change): for backward compatibility, can be removed if -// export_command_interfaces() method is removed -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector interfaces = impl_->export_state_interfaces(); -#pragma GCC diagnostic pop - // END: for backward compatibility - - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_state_interfaces(); - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto const & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(interface)); - } - return interface_ptrs; - // END: for backward compatibility -} - -const std::string & Sensor::get_name() const { return impl_->get_name(); } - -const std::string & Sensor::get_group_name() const { return impl_->get_group_name(); } - -const rclcpp_lifecycle::State & Sensor::get_lifecycle_state() const -{ - return impl_->get_lifecycle_state(); -} - -const rclcpp::Time & Sensor::get_last_read_time() const { return last_read_cycle_time_; } - -const HardwareComponentStatisticsCollector & Sensor::get_read_statistics() const -{ - return read_statistics_; -} - -return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_read_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_read(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - read_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_read_cycle_time_).seconds()); - } - last_read_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -std::recursive_mutex & Sensor::get_mutex() { return sensors_mutex_; } -} // namespace hardware_interface diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp deleted file mode 100644 index 8bb8ec4903..0000000000 --- a/hardware_interface/src/system.cpp +++ /dev/null @@ -1,417 +0,0 @@ -// Copyright 2020 - 2021 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. - -#include "hardware_interface/system.hpp" - -#include -#include -#include -#include - -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/lifecycle_helpers.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "hardware_interface/types/lifecycle_state_names.hpp" -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace hardware_interface -{ -using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - -System::System(std::unique_ptr impl) : impl_(std::move(impl)) {} - -System::System(System && other) noexcept -{ - std::lock_guard lock(other.system_mutex_); - impl_ = std::move(other.impl_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); -} - -const rclcpp_lifecycle::State & System::initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -{ - // This is done for backward compatibility with the old initialize method. -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - return this->initialize(system_info, logger, clock_interface->get_clock()); -#pragma GCC diagnostic pop -} - -const rclcpp_lifecycle::State & System::initialize( - const HardwareInfo & system_info, rclcpp::Logger logger, rclcpp::Clock::SharedPtr clock) -{ - hardware_interface::HardwareComponentParams params; - params.hardware_info = system_info; - params.logger = logger; - params.clock = clock; - return initialize(params); -} - -const rclcpp_lifecycle::State & System::initialize( - const hardware_interface::HardwareComponentParams & params) -{ - std::unique_lock lock(system_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN) - { - switch (impl_->init(params)) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::configure() -{ - std::unique_lock lock(system_mutex_); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_configure(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::cleanup() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - switch (impl_->on_cleanup(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::shutdown() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) - { - switch (impl_->on_shutdown(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::activate() -{ - std::unique_lock lock(system_mutex_); - last_read_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - last_write_cycle_time_ = rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED); - read_statistics_.reset_statistics(); - write_statistics_.reset_statistics(); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) - { - impl_->prepare_for_activation(); - switch (impl_->on_activate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->enable_introspection(true); - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::deactivate() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - switch (impl_->on_deactivate(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, lifecycle_state_names::INACTIVE)); - break; - case CallbackReturn::FAILURE: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, lifecycle_state_names::ACTIVE)); - break; - case CallbackReturn::ERROR: - impl_->set_lifecycle_state(error()); - break; - } - } - return impl_->get_lifecycle_state(); -} - -const rclcpp_lifecycle::State & System::error() -{ - std::unique_lock lock(system_mutex_); - impl_->enable_introspection(false); - if ( - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNKNOWN && - impl_->get_lifecycle_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) - { - switch (impl_->on_error(impl_->get_lifecycle_state())) - { - case CallbackReturn::SUCCESS: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, - lifecycle_state_names::UNCONFIGURED)); - break; - case CallbackReturn::FAILURE: - case CallbackReturn::ERROR: - impl_->set_lifecycle_state( - rclcpp_lifecycle::State( - lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, lifecycle_state_names::FINALIZED)); - break; - } - } - return impl_->get_lifecycle_state(); -} - -std::vector System::export_state_interfaces() -{ -// BEGIN (Handle export change): for backward compatibility, can be removed if -// export_command_interfaces() method is removed -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector interfaces = impl_->export_state_interfaces(); -#pragma GCC diagnostic pop - // END: for backward compatibility - - // If no StateInterfaces has been exported, this could mean: - // a) there is nothing to export -> on_export_state_interfaces() does return nothing as well - // b) default implementation for export_state_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_state_interfaces(); - } - - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto const & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(interface)); - } - return interface_ptrs; - // END: for backward compatibility -} - -std::vector System::export_command_interfaces() -{ -// BEGIN (Handle export change): for backward compatibility, can be removed if -// export_command_interfaces() method is removed -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wdeprecated-declarations" - std::vector interfaces = impl_->export_command_interfaces(); -#pragma GCC diagnostic pop - // END: for backward compatibility - - // If no CommandInterface has been exported, this could mean: - // a) there is nothing to export -> on_export_command_interfaces() does return nothing as well - // b) default implementation for export_command_interfaces() is used -> new functionality -> - // Framework exports and creates everything - if (interfaces.empty()) - { - return impl_->on_export_command_interfaces(); - } - // BEGIN (Handle export change): for backward compatibility, can be removed if - // export_command_interfaces() method is removed - std::vector interface_ptrs; - interface_ptrs.reserve(interfaces.size()); - for (auto & interface : interfaces) - { - interface_ptrs.emplace_back(std::make_shared(std::move(interface))); - } - return interface_ptrs; - // END: for backward compatibility -} - -return_type System::prepare_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->prepare_command_mode_switch(start_interfaces, stop_interfaces); -} - -return_type System::perform_command_mode_switch( - const std::vector & start_interfaces, - const std::vector & stop_interfaces) -{ - return impl_->perform_command_mode_switch(start_interfaces, stop_interfaces); -} - -const std::string & System::get_name() const { return impl_->get_name(); } - -const std::string & System::get_group_name() const { return impl_->get_group_name(); } - -const rclcpp_lifecycle::State & System::get_lifecycle_state() const -{ - return impl_->get_lifecycle_state(); -} - -const rclcpp::Time & System::get_last_read_time() const { return last_read_cycle_time_; } - -const rclcpp::Time & System::get_last_write_time() const { return last_write_cycle_time_; } - -const HardwareComponentStatisticsCollector & System::get_read_statistics() const -{ - return read_statistics_; -} - -const HardwareComponentStatisticsCollector & System::get_write_statistics() const -{ - return write_statistics_; -} - -return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_read_cycle_time_ = time; - return return_type::OK; - } - if ( - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || - impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_read(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - read_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_read_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - read_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_read_cycle_time_).seconds()); - } - last_read_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & period) -{ - if (lifecycleStateThatRequiresNoAction(impl_->get_lifecycle_state().id())) - { - last_write_cycle_time_ = time; - return return_type::OK; - } - // only call write in the active state - if (impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - const auto trigger_result = impl_->trigger_write(time, period); - if (trigger_result.result == return_type::ERROR) - { - error(); - } - if (trigger_result.successful) - { - if (trigger_result.execution_time.has_value()) - { - write_statistics_.execution_time->AddMeasurement( - static_cast(trigger_result.execution_time.value().count()) / 1.e3); - } - if (last_write_cycle_time_.get_clock_type() != RCL_CLOCK_UNINITIALIZED) - { - write_statistics_.periodicity->AddMeasurement( - 1.0 / (time - last_write_cycle_time_).seconds()); - } - last_write_cycle_time_ = time; - } - return trigger_result.result; - } - return return_type::OK; -} - -std::recursive_mutex & System::get_mutex() { return system_mutex_; } -} // namespace hardware_interface From 49f50f459162d322422f8c120806f27f6e33eb82 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 09:37:16 +0000 Subject: [PATCH 06/17] fixed tests --- .../test/test_component_interfaces.cpp | 34 +++++++++---------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index 25a97958ad..c67c0fd2f5 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -813,14 +813,13 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Read should work but write should not update the state because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity + EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -835,9 +834,10 @@ TEST(TestComponentInterfaces, dummy_actuator) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, - state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity + step * velocity_value, + state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0.0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -851,7 +851,7 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -939,17 +939,13 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read and Write are working because it is INACTIVE + // Read should work but write should not update the state because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ( - step * velocity_value, - state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0, - state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ(0.0, state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -964,9 +960,11 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - (10 + step) * velocity_value, + step * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step ? velocity_value : 0.0, + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -981,7 +979,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - 20 * velocity_value, + 10 * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity From 7c149a96e5aa0bff916c33a8c8d4e8e7bd3eb72f Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 09:47:49 +0000 Subject: [PATCH 07/17] updated cmakelists --- hardware_interface/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index 3b216d4108..e10f7ec16f 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -29,12 +29,9 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) endforeach() add_library(hardware_interface SHARED - # src/actuator.cpp src/component_parser.cpp src/resource_manager.cpp src/hardware_component.cpp - # src/sensor.cpp - # src/system.cpp src/lexical_casts.cpp ) target_compile_features(hardware_interface PUBLIC cxx_std_17) From 019fe33e103685786eb9148d8f97fe6ef5c44900 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Sun, 10 Aug 2025 10:57:20 +0000 Subject: [PATCH 08/17] fixed failing tests --- ...esource_manager_prepare_perform_switch.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index 5d7072be86..c4c04ac396 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -361,10 +361,10 @@ TEST_F( }; // System : UNCONFIGURED -// Actuator: ERROR +// Actuator: DEACTIVATED then WRITE_IGNORED TEST_F( ResourceManagerPreparePerformTest, - when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing) + when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive) { preconfigure_components( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", @@ -467,23 +467,25 @@ TEST_F( EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + // The component is INACTIVE, write does nothing, so no error is triggered. State remains + // INACTIVE. EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); - EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); + EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); }; // System : UNCONFIGURED From d07f5b36573addcaa569ff65de86478c06ef8b87 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Mon, 11 Aug 2025 13:05:37 +0000 Subject: [PATCH 09/17] Revert "fixed failing tests" This reverts commit 019fe33e103685786eb9148d8f97fe6ef5c44900. --- ...esource_manager_prepare_perform_switch.cpp | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp index c4c04ac396..5d7072be86 100644 --- a/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp +++ b/hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp @@ -361,10 +361,10 @@ TEST_F( }; // System : UNCONFIGURED -// Actuator: DEACTIVATED then WRITE_IGNORED +// Actuator: ERROR TEST_F( ResourceManagerPreparePerformTest, - when_actuator_deactivated_then_write_error_is_ignored_and_remains_inactive) + when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing) { preconfigure_components( lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured", @@ -467,25 +467,23 @@ TEST_F( EXPECT_EQ( status_map["TestSystemCommandModes"].state.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - // The component is INACTIVE, write does nothing, so no error is triggered. State remains - // INACTIVE. EXPECT_EQ( status_map["TestActuatorHardware"].state.id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.405, 1e-7); - EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); + EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys)); EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0); - EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.505, 1e-7); + EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7); }; // System : UNCONFIGURED From 1cf50d4d28123692d71cf89ced5e38dd57b03b8c Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Mon, 11 Aug 2025 13:06:06 +0000 Subject: [PATCH 10/17] Revert "fixed tests" This reverts commit 49f50f459162d322422f8c120806f27f6e33eb82. --- .../test/test_component_interfaces.cpp | 34 ++++++++++--------- 1 file changed, 18 insertions(+), 16 deletions(-) diff --git a/hardware_interface/test/test_component_interfaces.cpp b/hardware_interface/test/test_component_interfaces.cpp index c67c0fd2f5..25a97958ad 100644 --- a/hardware_interface/test/test_component_interfaces.cpp +++ b/hardware_interface/test/test_component_interfaces.cpp @@ -813,13 +813,14 @@ TEST(TestComponentInterfaces, dummy_actuator) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read should work but write should not update the state because it is INACTIVE + // Read and Write are working because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(0.0, state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ(0.0, state_interfaces[1]->get_optional().value()); // velocity + EXPECT_EQ( + step * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(step ? velocity_value : 0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -834,10 +835,9 @@ TEST(TestComponentInterfaces, dummy_actuator) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, - state_interfaces[0]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0.0, state_interfaces[1]->get_optional().value()); // velocity + (10 + step) * velocity_value, + state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(velocity_value, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -851,7 +851,7 @@ TEST(TestComponentInterfaces, dummy_actuator) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(10 * velocity_value, state_interfaces[0]->get_optional().value()); // position value + EXPECT_EQ(20 * velocity_value, state_interfaces[0]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[1]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); @@ -939,13 +939,17 @@ TEST(TestComponentInterfaces, dummy_actuator_default) EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state.id()); EXPECT_EQ(hardware_interface::lifecycle_state_names::INACTIVE, state.label()); - // Read should work but write should not update the state because it is INACTIVE + // Read and Write are working because it is INACTIVE for (auto step = 0u; step < 10; ++step) { ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); - EXPECT_EQ(0.0, state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ(0.0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ( + step * velocity_value, + state_interfaces[si_joint1_pos]->get_optional().value()); // position value + EXPECT_EQ( + step ? velocity_value : 0, + state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -960,11 +964,9 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - step * velocity_value, + (10 + step) * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value - EXPECT_EQ( - step ? velocity_value : 0.0, - state_interfaces[si_joint1_vel]->get_optional().value()); // velocity + EXPECT_EQ(velocity_value, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.write(TIME, PERIOD)); } @@ -979,7 +981,7 @@ TEST(TestComponentInterfaces, dummy_actuator_default) ASSERT_EQ(hardware_interface::return_type::OK, actuator_hw.read(TIME, PERIOD)); EXPECT_EQ( - 10 * velocity_value, + 20 * velocity_value, state_interfaces[si_joint1_pos]->get_optional().value()); // position value EXPECT_EQ(0, state_interfaces[si_joint1_vel]->get_optional().value()); // velocity From c37761f373814ac3d71ca44acbe73bcdf2272399 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Wed, 20 Aug 2025 10:38:22 +0000 Subject: [PATCH 11/17] added draft implementation --- .../hardware_component_interface.hpp | 122 ++++++++++++++++++ 1 file changed, 122 insertions(+) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index cb96f50fc9..2c765b17fb 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -24,6 +24,7 @@ #include #include +#include "control_msgs/msg/hardware_status.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -43,6 +44,8 @@ #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 { @@ -176,12 +179,112 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif params.hardware_info.name.c_str()); } + control_msgs::msg::HardwareStatus status_msg_template; + if (on_configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) + { + RCLCPP_ERROR(get_logger(), "User-defined 'on_configure_hardware_status_message' failed."); + return CallbackReturn::ERROR; + } + + if (!status_msg_template.hardware_device_states.empty()) + { + if (!hardware_component_node_) + { + RCLCPP_WARN( + get_logger(), + "Hardware status message was configured, but no node is available for the publisher. " + "Publishing will be disabled."); + } + else + { + try + { + hardware_status_publisher_ = + hardware_component_node_->create_publisher( + "~/hardware_status", rclcpp::SystemDefaultsQoS()); + + double publish_rate = 1.0; // Default to 1 Hz + // auto it = info_.hardware_parameters.find("status_publish_rate"); + // if (it != info_.hardware_parameters.end()) + // { + // try + // { + // publish_rate = hardware_interface::stod(it->second); + // } + // catch (const std::invalid_argument &) + // { + // RCLCPP_WARN( + // get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.", + // publish_rate); + // } + // } + + hardware_status_timer_ = hardware_component_node_->create_wall_timer( + std::chrono::duration(1.0 / publish_rate), + [this]() + { + std::optional msg_to_publish_opt; + hardware_status_box_.get(msg_to_publish_opt); + + if (msg_to_publish_opt.has_value() && hardware_status_publisher_) + { + control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); + msg.header.stamp = this->get_clock()->now(); + hardware_status_publisher_->publish(msg); + } + }); + hardware_status_box_.set(std::make_optional(status_msg_template)); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception during publisher/timer setup for hardware status: %s", + e.what()); + return CallbackReturn::ERROR; + } + } + } + hardware_interface::HardwareComponentInterfaceParams interface_params; interface_params.hardware_info = info_; interface_params.executor = params.executor; return on_init(interface_params); }; + /// User-overridable method to configure the structure of the HardwareStatus message. + /** + * To enable status publishing, override this method to pre-allocate the message structure + * and fill in static information like device IDs and interface names. This method is called + * once during the non-realtime `init()` phase. If the `hardware_device_states` vector is + * left empty, publishing will be disabled. + * + * \param[out] msg_template A reference to a HardwareStatus message to be configured. + * \returns CallbackReturn::SUCCESS if configured successfully, CallbackReturn::ERROR on failure. + */ + virtual CallbackReturn on_configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) + { + // Default implementation does nothing, disabling the feature. + (void)msg_template; + return CallbackReturn::SUCCESS; + } + + /// User-overridable method to fill the hardware status message with real-time data. + /** + * This real-time safe method is called by the framework within the `trigger_read()` loop. + * Override this method to populate the `value` fields of the pre-allocated message with the + * latest hardware states that were updated in your `read()` method. + * + * \param[in,out] msg The pre-allocated message to be filled with the latest values. + * \returns return_type::OK on success, return_type::ERROR on failure. + */ + virtual return_type on_update_hardware_status_message(control_msgs::msg::HardwareStatus & msg) + { + // Default implementation does nothing. + (void)msg; + return return_type::OK; + } + /// Initialization of the hardware interface from data parsed from the robot's URDF. /** * \param[in] hardware_info structure with data from URDF. @@ -490,6 +593,21 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); } + + if (hardware_status_publisher_) + { + auto status_msg_ptr = hardware_status_box_.get(); + if (status_msg_ptr) + { + if (on_update_hardware_status_message(*status_msg_ptr) != return_type::OK) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 1000, "Failed to update hardware status message for '%s'.", + info_.name.c_str()); + } + } + } + return status; } @@ -765,6 +883,10 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif protected: pal_statistics::RegistrationsRAII stats_registrations_; + std::shared_ptr> hardware_status_publisher_; + realtime_tools::RealtimeThreadSafeBox> + hardware_status_box_; + rclcpp::TimerBase::SharedPtr hardware_status_timer_; }; } // namespace hardware_interface From c10d77e0bbc8f44a7f0696621769659ccaf1e3fa Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 08:16:36 +0000 Subject: [PATCH 12/17] updated default publisher logic --- .../hardware_component_interface.hpp | 144 ++++++++++-------- 1 file changed, 77 insertions(+), 67 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 2c765b17fb..84c00a865b 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -179,70 +179,95 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif params.hardware_info.name.c_str()); } - control_msgs::msg::HardwareStatus status_msg_template; - if (on_configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) + double publish_rate = 0.0; + auto it = info_.hardware_parameters.find("status_publish_rate"); + if (it != info_.hardware_parameters.end()) { - RCLCPP_ERROR(get_logger(), "User-defined 'on_configure_hardware_status_message' failed."); - return CallbackReturn::ERROR; + try + { + publish_rate = hardware_interface::stod(it->second); + } + catch (const std::invalid_argument &) + { + RCLCPP_WARN( + get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.", + publish_rate); + } } - if (!status_msg_template.hardware_device_states.empty()) + if (publish_rate == 0.0) + { + RCLCPP_INFO( + get_logger(), + "`status_publish_rate` is set to 0.0, hardware status publisher will not be created."); + } + else { - if (!hardware_component_node_) + control_msgs::msg::HardwareStatus status_msg_template; + if (on_configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) { - RCLCPP_WARN( - get_logger(), - "Hardware status message was configured, but no node is available for the publisher. " - "Publishing will be disabled."); + RCLCPP_ERROR(get_logger(), "User-defined 'on_configure_hardware_status_message' failed."); + return CallbackReturn::ERROR; } - else + + if (!status_msg_template.hardware_device_states.empty()) { - try + if (!hardware_component_node_) { - hardware_status_publisher_ = - hardware_component_node_->create_publisher( - "~/hardware_status", rclcpp::SystemDefaultsQoS()); - - double publish_rate = 1.0; // Default to 1 Hz - // auto it = info_.hardware_parameters.find("status_publish_rate"); - // if (it != info_.hardware_parameters.end()) - // { - // try - // { - // publish_rate = hardware_interface::stod(it->second); - // } - // catch (const std::invalid_argument &) - // { - // RCLCPP_WARN( - // get_logger(), "Invalid 'status_publish_rate' parameter. Using default %.1f Hz.", - // publish_rate); - // } - // } - - hardware_status_timer_ = hardware_component_node_->create_wall_timer( - std::chrono::duration(1.0 / publish_rate), - [this]() - { - std::optional msg_to_publish_opt; - hardware_status_box_.get(msg_to_publish_opt); - - if (msg_to_publish_opt.has_value() && hardware_status_publisher_) - { - control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); - msg.header.stamp = this->get_clock()->now(); - hardware_status_publisher_->publish(msg); - } - }); - hardware_status_box_.set(std::make_optional(status_msg_template)); + RCLCPP_WARN( + get_logger(), + "Hardware status message was configured, but no node is available for the publisher. " + "Publisher will not be created."); } - catch (const std::exception & e) + else { - RCLCPP_ERROR( - get_logger(), "Exception during publisher/timer setup for hardware status: %s", - e.what()); - return CallbackReturn::ERROR; + try + { + hardware_status_publisher_ = + hardware_component_node_->create_publisher( + "~/hardware_status", rclcpp::SystemDefaultsQoS()); + + hardware_status_timer_ = hardware_component_node_->create_wall_timer( + std::chrono::duration(1.0 / publish_rate), + [this]() + { + std::optional msg_to_publish_opt; + hardware_status_box_.get(msg_to_publish_opt); + + if (msg_to_publish_opt.has_value() && hardware_status_publisher_) + { + control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); + if (on_update_hardware_status_message(msg) != return_type::OK) + { + RCLCPP_WARN_THROTTLE( + get_logger(), *clock_, 1000, + "User's on_update_hardware_status_message() failed for '%s'.", + info_.name.c_str()); + return; + } + msg.header.stamp = this->get_clock()->now(); + hardware_status_publisher_->publish(msg); + } + }); + hardware_status_box_.set(std::make_optional(status_msg_template)); + } + catch (const std::exception & e) + { + RCLCPP_ERROR( + get_logger(), "Exception during publisher/timer setup for hardware status: %s", + e.what()); + return CallbackReturn::ERROR; + } } } + else + { + RCLCPP_WARN( + get_logger(), + "`status_publish_rate` was set to a non-zero value, but no hardware status message was " + "configured. Publisher will not be created. Are you sure " + "on_configure_hardware_status_message() is set up properly?"); + } } hardware_interface::HardwareComponentInterfaceParams interface_params; @@ -593,21 +618,6 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif status.execution_time = std::chrono::duration_cast( std::chrono::steady_clock::now() - start_time); } - - if (hardware_status_publisher_) - { - auto status_msg_ptr = hardware_status_box_.get(); - if (status_msg_ptr) - { - if (on_update_hardware_status_message(*status_msg_ptr) != return_type::OK) - { - RCLCPP_WARN_THROTTLE( - get_logger(), *clock_, 1000, "Failed to update hardware status message for '%s'.", - info_.name.c_str()); - } - } - } - return status; } From 8dbe943ec476298ba098538692b8d443dbdc946a Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 08:28:45 +0000 Subject: [PATCH 13/17] updated apis --- .../hardware_component_interface.hpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp index 84c00a865b..80b603d1eb 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_interface.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_interface.hpp @@ -204,9 +204,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif else { control_msgs::msg::HardwareStatus status_msg_template; - if (on_configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) + if (configure_hardware_status_message(status_msg_template) != CallbackReturn::SUCCESS) { - RCLCPP_ERROR(get_logger(), "User-defined 'on_configure_hardware_status_message' failed."); + RCLCPP_ERROR(get_logger(), "User-defined 'configure_hardware_status_message' failed."); return CallbackReturn::ERROR; } @@ -237,11 +237,11 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif if (msg_to_publish_opt.has_value() && hardware_status_publisher_) { control_msgs::msg::HardwareStatus & msg = msg_to_publish_opt.value(); - if (on_update_hardware_status_message(msg) != return_type::OK) + if (update_hardware_status_message(msg) != return_type::OK) { RCLCPP_WARN_THROTTLE( get_logger(), *clock_, 1000, - "User's on_update_hardware_status_message() failed for '%s'.", + "User's update_hardware_status_message() failed for '%s'.", info_.name.c_str()); return; } @@ -266,7 +266,7 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif get_logger(), "`status_publish_rate` was set to a non-zero value, but no hardware status message was " "configured. Publisher will not be created. Are you sure " - "on_configure_hardware_status_message() is set up properly?"); + "configure_hardware_status_message() is set up properly?"); } } @@ -286,11 +286,10 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \param[out] msg_template A reference to a HardwareStatus message to be configured. * \returns CallbackReturn::SUCCESS if configured successfully, CallbackReturn::ERROR on failure. */ - virtual CallbackReturn on_configure_hardware_status_message( - control_msgs::msg::HardwareStatus & msg_template) + virtual CallbackReturn configure_hardware_status_message( + control_msgs::msg::HardwareStatus & /*msg_template*/) { // Default implementation does nothing, disabling the feature. - (void)msg_template; return CallbackReturn::SUCCESS; } @@ -303,10 +302,9 @@ class HardwareComponentInterface : public rclcpp_lifecycle::node_interfaces::Lif * \param[in,out] msg The pre-allocated message to be filled with the latest values. * \returns return_type::OK on success, return_type::ERROR on failure. */ - virtual return_type on_update_hardware_status_message(control_msgs::msg::HardwareStatus & msg) + virtual return_type update_hardware_status_message(control_msgs::msg::HardwareStatus & /*msg*/) { // Default implementation does nothing. - (void)msg; return return_type::OK; } From d9cd3e733a4f74fa83138a916f277c6a6c752f9d Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 19:26:08 +0000 Subject: [PATCH 14/17] added rough draft for documentation --- .../doc/writing_new_hardware_component.rst | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 6f926988d3..9500a15b6e 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -52,9 +52,13 @@ The following is a step-by-step guide to create source files, basic tests, and c A common requirement for a hardware component is to publish status or diagnostic information without interfering with the real-time control loop. - This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are two primary ways to achieve this. + This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are three primary ways to achieve this. - **Method 1: Using the Framework-Managed Node (Recommended & Simplest)** + **Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for ``HardwareStatus`` Messages)** + + Refer :ref:`Framework Managed Publisher ` + + **Method 2: Using the Framework-Managed Node (Recommended & Simplest for Custom Messages)** The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it. @@ -75,7 +79,7 @@ The following is a step-by-step guide to create source files, basic tests, and c }); } - **Method 2: Using the Executor from `HardwareComponentInterfaceParams`** + **Method 3: Using the Executor from `HardwareComponentInterfaceParams`** For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor. @@ -150,6 +154,12 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. +.. _framework_managed_publisher: + + #. (optional) Implement ``configure_hardware_status_message`` and ``update_hardware_status_message`` method to publish the framework supported hardware status reporting through ``HardwareStatus`` messages. + + * The framework internally creates a dedicated ROS 2 publisher for each hardware component with ``HardwareStatus`` message type. + #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. For this you will need to include the ``"pluginlib/class_list_macros.hpp"`` header. From 78bc6acca78b50ec54cc6b9e04a67c8d18ded5d6 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Fri, 22 Aug 2025 20:09:50 +0000 Subject: [PATCH 15/17] updated documentation --- .../doc/writing_new_hardware_component.rst | 138 +++++++++++++----- 1 file changed, 102 insertions(+), 36 deletions(-) diff --git a/hardware_interface/doc/writing_new_hardware_component.rst b/hardware_interface/doc/writing_new_hardware_component.rst index 9500a15b6e..6d4fd8f1d5 100644 --- a/hardware_interface/doc/writing_new_hardware_component.rst +++ b/hardware_interface/doc/writing_new_hardware_component.rst @@ -54,56 +54,56 @@ The following is a step-by-step guide to create source files, basic tests, and c This allows you to add any standard ROS 2 component (publishers, subscribers, services, timers) to your hardware interface without compromising real-time performance. There are three primary ways to achieve this. - **Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for ``HardwareStatus`` Messages)** + **Method 1: Using the Framework-Managed Publisher (Recommended & Simplest for HardwareStatus Messages)** - Refer :ref:`Framework Managed Publisher ` + Refer :ref:`Framework Managed Publisher ` **Method 2: Using the Framework-Managed Node (Recommended & Simplest for Custom Messages)** - The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it. + The framework internally creates a dedicated ROS 2 node for each hardware component. Your hardware plugin can then get a handle to this node and use it. - #. **Access and using the Default Node**: You can get a ``shared_ptr`` to the node by calling the ``get_node()`` method and use it just like any other ``rclcpp::Node::SharedPtr`` to create publishers, timers, etc. + #. **Access and using the Default Node**: You can get a ``shared_ptr`` to the node by calling the ``get_node()`` method and use it just like any other ``rclcpp::Node::SharedPtr`` to create publishers, timers, etc. - .. code-block:: cpp + .. code-block:: cpp - // Continuing inside on_configure() - if (get_node()) - { - my_publisher_ = get_node()->create_publisher("~/status", 10); - - using namespace std::chrono_literals; - my_timer_ = get_node()->create_wall_timer(1s, [this]() { - std_msgs::msg::String msg; - msg.data = "Hardware status update!"; - my_publisher_->publish(msg); - }); - } + // Continuing inside on_configure() + if (get_node()) + { + my_publisher_ = get_node()->create_publisher("~/status", 10); + + using namespace std::chrono_literals; + my_timer_ = get_node()->create_wall_timer(1s, [this]() { + std_msgs::msg::String msg; + msg.data = "Hardware status update!"; + my_publisher_->publish(msg); + }); + } **Method 3: Using the Executor from `HardwareComponentInterfaceParams`** - For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor. + For more advanced use cases where you need direct control over node creation, the ``on_init`` method can be configured to receive a ``HardwareComponentInterfaceParams`` struct. This struct contains a ``weak_ptr`` to the ``ControllerManager``'s executor. - #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``. + #. **Update ``on_init`` Signature**: First, your hardware interface must override the ``on_init`` version that takes ``HardwareComponentInterfaceParams``. - .. code-block:: cpp + .. code-block:: cpp - // In your .hpp - hardware_interface::CallbackReturn on_init( - const hardware_interface::HardwareComponentInterfaceParams & params) override; + // In your .hpp + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareComponentInterfaceParams & params) override; - #. **Lock and Use the Executor**: Inside ``on_init``, you must safely "lock" the ``weak_ptr`` to get a usable ``shared_ptr``. You can then create your own node and add it to the executor. + #. **Lock and Use the Executor**: Inside ``on_init``, you must safely "lock" the ``weak_ptr`` to get a usable ``shared_ptr``. You can then create your own node and add it to the executor. - .. code-block:: cpp + .. code-block:: cpp - // In your .cpp, inside on_init(params) - if (auto locked_executor = params.executor.lock()) - { - my_custom_node_ = std::make_shared("my_custom_node"); - locked_executor->add_node(my_custom_node_->get_node_base_interface()); - // ... create publishers/timers on my_custom_node_ ... - } + // In your .cpp, inside on_init(params) + if (auto locked_executor = params.executor.lock()) + { + my_custom_node_ = std::make_shared("my_custom_node"); + locked_executor->add_node(my_custom_node_->get_node_base_interface()); + // ... create publishers/timers on my_custom_node_ ... + } - For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in example 17. + For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 `. #. Write the ``on_configure`` method where you usually setup the communication to the hardware and set everything up so that the hardware can be activated. @@ -154,11 +154,77 @@ The following is a step-by-step guide to create source files, basic tests, and c #. Implement ``write`` method that commands the hardware based on the values stored in internal variables defined in ``export_command_interfaces``. -.. _framework_managed_publisher: + #. (optional) **Framework Managed Publisher** + + .. _framework_managed_publisher: + + Implement ``configure_hardware_status_message`` and ``update_hardware_status_message`` methods to publish the framework-supported hardware status reporting through ``control_msgs/msg/HardwareStatus`` messages: + + * **`configure_hardware_status_message`**: This non-realtime method is called once during initialization. You must override it to define the **static structure** of your status message. This includes setting the ``hardware_id``, resizing the ``hardware_device_states`` vector, and for each device, resizing its specific status vectors (e.g., ``generic_hardware_status``, ``canopen_states``) and populating static fields like ``device_id`` and interface ``name``. Pre-allocating the message structure here is crucial for real-time safety. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::CallbackReturn configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg_template) override; + + // In your .cpp + hardware_interface::CallbackReturn MyHardware::configure_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) + { + msg.hardware_id = get_hardware_info().name; + msg.hardware_device_states.resize(get_hardware_info().joints.size()); + + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + msg.hardware_device_states[i].device_id = get_hardware_info().joints[i].name; + // This example uses one generic status per joint + msg.hardware_device_states[i].generic_hardware_status.resize(1); + } + return hardware_interface::CallbackReturn::SUCCESS; + } + + * **`update_hardware_status_message`**: This real-time safe method is called from the framework's timer callback. You must override it to **fill in the dynamic values** of the pre-structured message. This typically involves copying your internal state variables (updated in your `read()` method) into the fields of the message. This method must be fast and non-allocating. + + .. code-block:: cpp + + // In your .hpp + hardware_interface::return_type update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) override; + + // In your .cpp + hardware_interface::return_type MyHardware::update_hardware_status_message( + control_msgs::msg::HardwareStatus & msg) + { + for (size_t i = 0; i < get_hardware_info().joints.size(); ++i) + { + auto & generic_status = msg.hardware_device_states[i].generic_hardware_status; + // Example: Map internal state to a standard status field + if (std::abs(hw_positions_[i]) > joint_limits_[i].max_position) + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_ERROR; + } + else + { + generic_status.health_status = control_msgs::msg::GenericState::HEALTH_OK; + } + } + return hardware_interface::return_type::OK; + } + + * **Enable in URDF**: Finally, to activate the publisher, add the ``status_publish_rate`` parameter to your ```` tag in the URDF. Setting it to 0.0 disables the feature. + + .. code-block:: xml - #. (optional) Implement ``configure_hardware_status_message`` and ``update_hardware_status_message`` method to publish the framework supported hardware status reporting through ``HardwareStatus`` messages. + + + my_package/MyHardware + 20.0 + + ... + - * The framework internally creates a dedicated ROS 2 publisher for each hardware component with ``HardwareStatus`` message type. + For a complete, working implementation that uses the framework-managed node to publish diagnostic messages, see the demo in :ref:`Example 17 `. #. IMPORTANT: At the end of your file after the namespace is closed, add the ``PLUGINLIB_EXPORT_CLASS`` macro. From 381ecb773f21e1ee144c05928fbe5c9bb1b7feea Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Wed, 27 Aug 2025 10:29:15 +0000 Subject: [PATCH 16/17] added view hardware status verb --- ros2controlcli/package.xml | 1 + .../verb/view_hardware_status.py | 177 ++++++++++++++++++ ros2controlcli/setup.py | 1 + 3 files changed, 179 insertions(+) create mode 100644 ros2controlcli/ros2controlcli/verb/view_hardware_status.py diff --git a/ros2controlcli/package.xml b/ros2controlcli/package.xml index e5a2c67119..94c3c77cdd 100644 --- a/ros2controlcli/package.xml +++ b/ros2controlcli/package.xml @@ -18,6 +18,7 @@ ros2param controller_manager controller_manager_msgs + control_msgs rosidl_runtime_py python3-pygraphviz diff --git a/ros2controlcli/ros2controlcli/verb/view_hardware_status.py b/ros2controlcli/ros2controlcli/verb/view_hardware_status.py new file mode 100644 index 0000000000..4454c3fe7d --- /dev/null +++ b/ros2controlcli/ros2controlcli/verb/view_hardware_status.py @@ -0,0 +1,177 @@ +# Copyright 2025 ROS-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. + +from functools import partial +import datetime + +import rclpy +from ros2cli.node.direct import add_arguments +from ros2cli.node.strategy import NodeStrategy +from ros2cli.verb import VerbExtension +from ros2topic.api import get_topic_names_and_types + +from control_msgs.msg import HardwareStatus +from controller_manager import bcolors + +from rosidl_runtime_py import message_to_yaml + +_DISCOVERY_THRESHOLD = 10 + + +class ViewHardwareStatusVerb(VerbExtension): + """Echo hardware status messages with filtering capabilities.""" + + def __init__(self): + super().__init__() + self.found_hardware_ids = set() + self.found_device_ids = set() + self.message_count = 0 + self.discovery_complete = False + + def add_arguments(self, parser, cli_name): + add_arguments(parser) + parser.add_argument( + "-i", + "--hardware-id", + dest="hardware_id", + help="Filter by a specific hardware component ID.", + ) + parser.add_argument( + "-d", + "--device-id", + dest="device_id", + help="Filter by a specific device ID within a hardware component.", + ) + + def _on_message(self, msg, args): + self.message_count += 1 + self.found_hardware_ids.add(msg.hardware_id) + for device_state in msg.hardware_device_states: + self.found_device_ids.add(device_state.device_id) + + if not self.discovery_complete and self.message_count >= _DISCOVERY_THRESHOLD: + self.discovery_complete = True + + if args.hardware_id and args.hardware_id not in self.found_hardware_ids: + print( + f"\n{bcolors.FAIL}Error: Hardware ID '{args.hardware_id}' not found.{bcolors.ENDC}" + ) + if self.found_hardware_ids: + print(f"{bcolors.OKBLUE}Available Hardware IDs:{bcolors.ENDC}") + for hw_id in sorted(self.found_hardware_ids): + print(f"\t{hw_id}") + else: + print(f"{bcolors.WARNING}No hardware IDs discovered.{bcolors.ENDC}") + rclpy.shutdown() + return + + if args.device_id and args.device_id not in self.found_device_ids: + print( + f"\n{bcolors.FAIL}Error: Device ID '{args.device_id}' not found.{bcolors.ENDC}" + ) + if self.found_device_ids: + print(f"{bcolors.OKBLUE}Available Device IDs:{bcolors.ENDC}") + for dev_id in sorted(self.found_device_ids): + print(f"\t{dev_id}") + else: + print(f"{bcolors.WARNING}No device IDs discovered.{bcolors.ENDC}") + rclpy.shutdown() + return + + if args.hardware_id and msg.hardware_id != args.hardware_id: + return + + if args.device_id and not any( + d.device_id == args.device_id for d in msg.hardware_device_states + ): + return + + try: + dt_object = datetime.datetime.fromtimestamp(msg.header.stamp.sec) + nano_str = f"{msg.header.stamp.nanosec:09d}" + timestamp = f"{dt_object.strftime('%H:%M:%S')}.{nano_str[:3]}" + except (ValueError, OSError): + timestamp = f"{msg.header.stamp.sec}.{msg.header.stamp.nanosec:09d}" + + print( + f"{bcolors.OKGREEN}Hardware ID: {bcolors.ENDC}{msg.hardware_id} ({bcolors.WARNING}stamp: {timestamp}{bcolors.ENDC})" + ) + + for device_state in msg.hardware_device_states: + if args.device_id and device_state.device_id != args.device_id: + continue + + print(f" {bcolors.OKCYAN}Device ID: {bcolors.ENDC}{device_state.device_id}") + + state_types = [ + ("Generic Hardware States", device_state.hardware_status), + ("CANopen States", device_state.canopen_states), + ("EtherCAT States", device_state.ethercat_states), + ("VDA5050 States", device_state.vda5050_states), + ] + + any_state_printed = False + for title, states in state_types: + if states: + any_state_printed = True + print(f" {bcolors.OKBLUE}{title}:{bcolors.ENDC}") + for state in states: + print(" -") + yaml_str = message_to_yaml(state, flow_style=False) + indented_str = "\n".join( + [f" {line}" for line in yaml_str.splitlines()] + ) + print(indented_str) + + if not any_state_printed: + print(f" {bcolors.FAIL}Status: No specific states reported{bcolors.ENDC}") + + print("---") + + def main(self, *, args): + with NodeStrategy(args).direct_node as node: + topic_names_and_types = get_topic_names_and_types( + node=node, include_hidden_topics=True + ) + + status_topics = sorted( + [ + name + for name, types in topic_names_and_types + if name.endswith("/hardware_status") + and "control_msgs/msg/HardwareStatus" in types + ] + ) + + if not status_topics: + print( + f"{bcolors.FAIL}No topics of type 'control_msgs/msg/HardwareStatus' found.{bcolors.ENDC}" + ) + return 1 + + print(f"{bcolors.OKBLUE}Subscribing to the following topics:{bcolors.ENDC}") + for topic in status_topics: + print(f"\t{topic}") + print("---") + + _ = [ + node.create_subscription( + HardwareStatus, topic, partial(self._on_message, args=args), 10 + ) + for topic in status_topics + ] + + rclpy.spin(node) + + return 0 diff --git a/ros2controlcli/setup.py b/ros2controlcli/setup.py index a87558fcaf..066b1447ca 100644 --- a/ros2controlcli/setup.py +++ b/ros2controlcli/setup.py @@ -69,6 +69,7 @@ ros2controlcli.verb.set_hardware_component_state:SetHardwareComponentStateVerb", "switch_controllers = ros2controlcli.verb.switch_controllers:SwitchControllersVerb", "unload_controller = ros2controlcli.verb.unload_controller:UnloadControllerVerb", + "view_hardware_status = ros2controlcli.verb.view_hardware_status:ViewHardwareStatusVerb", ], }, ) From 17cdeaa01f1d702c3c6d93427fd53d1a03174fe2 Mon Sep 17 00:00:00 2001 From: Soham Patil Date: Wed, 27 Aug 2025 10:42:22 +0000 Subject: [PATCH 17/17] update documentation --- ros2controlcli/doc/userdoc.rst | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/ros2controlcli/doc/userdoc.rst b/ros2controlcli/doc/userdoc.rst index f6eb6cdc6a..d550dda158 100644 --- a/ros2controlcli/doc/userdoc.rst +++ b/ros2controlcli/doc/userdoc.rst @@ -20,6 +20,7 @@ Currently supported commands are - ros2 control switch_controllers - ros2 control unload_controller - ros2 control view_controller_chains + - ros2 control view_hardware_status list_controllers @@ -364,3 +365,24 @@ view_controller_chains --include-hidden-nodes Consider hidden nodes as well --ros-args ... Pass arbitrary arguments to the executable + + +view_hardware_status +---------------------- + +.. code-block:: console + + $ ros2 control view_hardware_status -h + usage: ros2 control view_hardware_status [-h] [--spin-time SPIN_TIME] [-s] [-i HARDWARE_ID] [-d DEVICE_ID] + + Echo hardware status messages with filtering capabilities + + options: + -h, --help show this help message and exit + --spin-time SPIN_TIME + Spin time in seconds to wait for discovery (only applies when not using an already running daemon) + -s, --use-sim-time Enable ROS simulation time + -i HARDWARE_ID, --hardware-id HARDWARE_ID + Filter by a specific hardware component ID. + -d DEVICE_ID, --device-id DEVICE_ID + Filter by a specific device ID within a hardware component.