From 960cd942fa904935abed39adacc006d2831caa24 Mon Sep 17 00:00:00 2001 From: im23123v Date: Mon, 3 Mar 2025 00:06:30 +0530 Subject: [PATCH 1/2] Improved docstring consistency across the codebase --- .vscode/settings.json | 78 ++ .../chainable_controller_interface.hpp | 106 +-- .../controller_interface_base.hpp | 163 ++-- .../include/controller_interface/helpers.hpp | 26 +- .../force_torque_sensor.hpp | 70 +- .../semantic_components/imu_sensor.hpp | 48 +- .../semantic_components/led_rgb_device.hpp | 22 +- .../semantic_components/pose_sensor.hpp | 36 +- .../semantic_components/range_sensor.hpp | 11 +- .../semantic_component_command_interface.hpp | 52 +- .../semantic_component_interface.hpp | 53 +- .../controller_manager/controller_manager.hpp | 373 +++++---- .../controller_manager/controller_spec.hpp | 7 +- .../hardware_interface/component_parser.hpp | 12 +- .../hardware_interface/controller_info.hpp | 6 +- .../include/hardware_interface/handle.hpp | 14 +- .../hardware_interface/resource_manager.hpp | 710 ++++++++++-------- .../hardware_interface/sensor_interface.hpp | 266 ++++--- .../hardware_interface/system_interface.hpp | 347 +++++---- .../mock_components/generic_system.hpp | 16 +- hardware_interface/src/component_parser.cpp | 221 +++--- hardware_interface/src/resource_manager.cpp | 90 ++- .../joint_limits/joint_limiter_interface.hpp | 79 +- .../joint_limits/joint_limits_rosparam.hpp | 129 ++-- .../joint_limits/joint_limits_urdf.hpp | 16 +- .../four_bar_linkage_transmission.hpp | 2 +- .../simple_transmission.hpp | 14 +- .../simple_transmission_loader.hpp | 2 +- .../transmission_interface/transmission.hpp | 68 +- .../transmission_loader.hpp | 2 +- 30 files changed, 1769 insertions(+), 1270 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000000..6b3935bbde --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,78 @@ +{ + "files.associations": { + "vector": "cpp", + "memory": "cpp", + "mutex": "cpp", + "shared_mutex": "cpp", + "xstring": "cpp", + "array": "cpp", + "algorithm": "cpp", + "atomic": "cpp", + "bit": "cpp", + "cctype": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "compare": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstddef": "cpp", + "cstdint": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "exception": "cpp", + "format": "cpp", + "forward_list": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "ios": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "iterator": "cpp", + "limits": "cpp", + "list": "cpp", + "locale": "cpp", + "map": "cpp", + "new": "cpp", + "optional": "cpp", + "ostream": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "set": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "string": "cpp", + "system_error": "cpp", + "thread": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "typeinfo": "cpp", + "unordered_map": "cpp", + "utility": "cpp", + "variant": "cpp", + "xfacet": "cpp", + "xhash": "cpp", + "xiosbase": "cpp", + "xlocale": "cpp", + "xlocbuf": "cpp", + "xlocinfo": "cpp", + "xlocmes": "cpp", + "xlocmon": "cpp", + "xlocnum": "cpp", + "xloctime": "cpp", + "xmemory": "cpp", + "xstddef": "cpp", + "xtr1common": "cpp", + "xtree": "cpp", + "xutility": "cpp" + } +} \ No newline at end of file diff --git a/controller_interface/include/controller_interface/chainable_controller_interface.hpp b/controller_interface/include/controller_interface/chainable_controller_interface.hpp index 52aa47d0fd..832e726467 100644 --- a/controller_interface/include/controller_interface/chainable_controller_interface.hpp +++ b/controller_interface/include/controller_interface/chainable_controller_interface.hpp @@ -25,14 +25,14 @@ namespace controller_interface { -/// Virtual class to implement when integrating a controller that can be preceded by other -/// controllers. /** - * Specialization of ControllerInterface class to force implementation of methods specific for - * "chainable" controller, i.e., controller that can be preceded by an another controller, for - * example inner controller of an control cascade. + * @brief Virtual class for integrating a controller that can be preceded by other controllers. * + * This is a specialization of the ControllerInterface class that enforces the implementation + * of methods specific to "chainable" controllers. A chainable controller can be preceded + * by another controller, such as an inner controller in a control cascade. */ + class ChainableControllerInterface : public ControllerInterfaceBase { public: @@ -62,63 +62,69 @@ class ChainableControllerInterface : public ControllerInterfaceBase bool is_in_chained_mode() const final; protected: - /// Virtual method that each chainable controller should implement to export its read-only - /// chainable interfaces. - /** - * Each chainable controller implements this methods where all its state(read only) interfaces are - * exported. The method has the same meaning as `export_state_interfaces` method from - * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. - * - * \returns list of StateInterfaces that other controller can use as their inputs. - */ +/** + * @brief Virtual method to export read-only chainable interfaces. + * + * Each chainable controller must implement this method to export all its state (read-only) + * interfaces. This method serves the same purpose as the `export_state_interfaces` method + * in `hardware_interface::SystemInterface` or `hardware_interface::ActuatorInterface`. + * + * @return List of StateInterfaces that other controllers can use as inputs. + */ + virtual std::vector on_export_state_interfaces(); - /// Virtual method that each chainable controller should implement to export its read/write - /// chainable interfaces. /** - * Each chainable controller implements this methods where all input (command) interfaces are - * exported. The method has the same meaning as `export_command_interface` method from - * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface. - * - * \returns list of CommandInterfaces that other controller can use as their outputs. - */ + * @brief Virtual method to export read/write chainable interfaces. + * + * Each chainable controller must implement this method to export all input (command) interfaces. + * This method serves the same purpose as the `export_command_interface` method in + * `hardware_interface::SystemInterface` or `hardware_interface::ActuatorInterface`. + * + * @return List of CommandInterfaces that other controllers can use as their outputs. + */ + virtual std::vector on_export_reference_interfaces(); - /// Virtual method that each chainable controller should implement to switch chained mode. - /** - * Each chainable controller implements this methods to switch between "chained" and "external" - * mode. In "chained" mode all external interfaces like subscriber and service servers are - * disabled to avoid potential concurrency in input commands. - * - * \param[in] flag marking a switch to or from chained mode. - * - * \returns true if controller successfully switched between "chained" and "external" mode. - * \default returns true so the method don't have to be overridden if controller can always switch - * chained mode. - */ + /** + * @brief Virtual method to switch the chained mode of a chainable controller. + * + * Each chainable controller must implement this method to switch between "chained" and "external" + * modes. In "chained" mode, all external interfaces, such as subscribers and service servers, + * are disabled to prevent potential concurrency in input commands. + * + * @param[in] flag Indicates whether to switch to or from chained mode. + * @return True if the controller successfully switched between "chained" and "external" modes. + * @note The default implementation returns true, so this method does not need to be overridden + * if the controller can always switch modes. + */ + virtual bool on_set_chained_mode(bool chained_mode); - /// Update reference from input topics when not in chained mode. /** - * Each chainable controller implements this method to update reference from subscribers when not - * in chained mode. - * - * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. - */ + * @brief Update the reference from input topics when not in chained mode. + * + * Each chainable controller must implement this method to update the reference + * from subscribers when not operating in chained mode. + * + * @return `return_type::OK` if the update is successful, otherwise `return_type::ERROR`. + */ + virtual return_type update_reference_from_subscribers( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; - /// Execute calculations of the controller and update command interfaces. /** - * Update method for chainable controllers. - * In this method is valid to assume that \reference_interfaces_ hold the values for calculation - * of the commands in the current control step. - * This means that this method is called after \update_reference_from_subscribers if controller is - * not in chained mode. - * - * \returns return_type::OK if calculation and writing of interface is successfully, otherwise - * return_type::ERROR. - */ + * @brief Execute controller calculations and update command interfaces. + * + * This is the update method for chainable controllers. It is valid to assume that + * `reference_interfaces_` hold the values needed for command calculations in the current + * control step. This method is called after `update_reference_from_subscribers` if the controller + * is not in chained mode. + * + * @return `return_type::OK` if the calculation and writing to the interface are successful, + * otherwise `return_type::ERROR`. + */ + virtual return_type update_and_write_commands( const rclcpp::Time & time, const rclcpp::Duration & period) = 0; diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 0f76713eae..7d3e1f0956 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -40,12 +40,15 @@ enum class return_type : std::uint8_t ERROR = 1, }; -/// Indicating which interfaces are to be claimed. /** - * One might either claim all available command/state interfaces, - * specifying a set of individual interfaces, - * or none at all. + * @brief Indicate which interfaces are to be claimed. + * + * A controller may choose to: + * - Claim all available command/state interfaces. + * - Specify a set of individual interfaces to be claimed. + * - Claim no interfaces at all. */ + enum class interface_configuration_type : std::uint8_t { ALL = 0, @@ -102,63 +105,80 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy virtual ~ControllerInterfaceBase(); - /// Get configuration for controller's required command interfaces. /** - * Method used by the controller_manager to get the set of command interfaces used by the - * controller. Each controller can use individual method to determine interface names that in - * simples case have the following format: `/`. The method is called only in - * `inactive` or `active` state, i.e., `on_configure` has to be called first. The configuration is - * used to check if controller can be activated and to claim interfaces from hardware. The claimed - * interfaces are populated in the \ref ControllerInterfaceBase::command_interfaces_ - * "command_interfaces_" member. - * - * \returns configuration of command interfaces. - */ + * @brief Retrieve the configuration for the controller's required command interfaces. + * + * This method is used by the `controller_manager` to obtain the set of command interfaces + * utilized by the controller. Each controller may use its own method to determine interface + * names, which in the simplest case follow the format: `/`. + * + * This method is called only when the controller is in the `inactive` or `active` state, + * meaning `on_configure` must be called first. The configuration is used to verify whether + * the controller can be activated and to claim interfaces from the hardware. + * + * The claimed interfaces are populated in the + * `ControllerInterfaceBase::command_interfaces_` member. + * + * @return Configuration of command interfaces. + */ + virtual InterfaceConfiguration command_interface_configuration() const = 0; - /// Get configuration for controller's required state interfaces. /** - * Method used by the controller_manager to get the set of state interface used by the controller. - * Each controller can use individual method to determine interface names that in simples case - * have the following format: `/`. - * The method is called only in `inactive` or `active` state, i.e., `on_configure` has to be - * called first. - * The configuration is used to check if controller can be activated and to claim interfaces from - * hardware. - * The claimed interfaces are populated in the - * \ref ControllerInterfaceBase::state_interfaces_ "state_interfaces_" member. - * - * \returns configuration of state interfaces. - */ + * @brief Retrieve the configuration for the controller's required state interfaces. + * + * This method is used by the `controller_manager` to obtain the set of state interfaces + * utilized by the controller. Each controller may use its own method to determine interface + * names, which in the simplest case follow the format: `/`. + * + * This method is called only when the controller is in the `inactive` or `active` state, + * meaning `on_configure` must be called first. The configuration is used to verify whether + * the controller can be activated and to claim interfaces from the hardware. + * + * The claimed interfaces are populated in the + * `ControllerInterfaceBase::state_interfaces_` member. + * + * @return Configuration of state interfaces. + */ + virtual InterfaceConfiguration state_interface_configuration() const = 0; - /// Method that assigns the Loaned interfaces to the controller. /** - * Method used by the controller_manager to assign the interfaces to the controller. - * \note When this method is overridden, the user has to also implement the `release_interfaces` - * method by overriding it to release the interfaces. - * - * \param[in] command_interfaces vector of command interfaces to be assigned to the controller. - * \param[in] state_interfaces vector of state interfaces to be assigned to the controller. - */ + * @brief Assign loaned interfaces to the controller. + * + * This method is used by the `controller_manager` to assign command and state interfaces + * to the controller. + * + * @note When overriding this method, the user must also override the `release_interfaces` + * method to properly release the assigned interfaces. + * + * @param[in] command_interfaces Vector of command interfaces to be assigned to the controller. + * @param[in] state_interfaces Vector of state interfaces to be assigned to the controller. + */ + virtual void assign_interfaces( std::vector && command_interfaces, std::vector && state_interfaces); - /// Method that releases the Loaned interfaces from the controller. - /** - * Method used by the controller_manager to release the interfaces from the controller. - */ + /** + * @brief Release loaned interfaces from the controller. + * + * This method is used by the `controller_manager` to release the interfaces assigned + * to the controller. + */ + virtual void release_interfaces(); return_type init( const std::string & controller_name, const std::string & urdf, unsigned int cm_update_rate, const std::string & node_namespace, const rclcpp::NodeOptions & node_options); - /// Custom configure method to read additional parameters for controller-nodes - /* - * Override default implementation for configure of LifecycleNode to get parameters. - */ + /** + * @brief Custom configure method to read additional parameters for controller nodes. + * + * Overrides the default `LifecycleNode` configuration method to retrieve parameters. + */ + const rclcpp_lifecycle::State & configure(); /// Extending interface with initialization method which is individual for each controller @@ -225,13 +245,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy return node_options; } - /// Declare and initialize a parameter with a type. /** - * - * Wrapper function for templated node's declare_parameter() which checks if - * parameter is already declared. - * For use in all components that inherit from ControllerInterfaceBase - */ + * @brief Declare and initialize a parameter with a specified type. + * + * Wrapper function for the templated `declare_parameter()` method of the node, + * which checks if the parameter is already declared. + * + * This function is intended for use in all components that inherit from + * `ControllerInterfaceBase`. + */ + template auto auto_declare(const std::string & name, const ParameterT & default_value) { @@ -248,12 +271,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy // Methods for chainable controller types with default values so we can put all controllers into // one list in Controller Manager - /// Get information if a controller is chainable. /** - * Get information if a controller is chainable. - * - * \returns true is controller is chainable and false if it is not. - */ + * @brief Check whether the controller is chainable. + * + * Determines if the controller supports chaining with other controllers. + * + * @return `true` if the controller is chainable, `false` otherwise. + */ + virtual bool is_chainable() const = 0; /** @@ -284,14 +309,16 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy */ virtual bool set_chained_mode(bool chained_mode) = 0; - /// Get information if a controller is currently in chained mode. /** - * Get information about controller if it is currently used in chained mode. In chained mode only - * internal interfaces are available and all subscribers are expected to be disabled. This - * prevents concurrent writing to controller's inputs from multiple sources. - * - * \returns true is controller is in chained mode and false if it is not. - */ + * @brief Check whether the controller is currently in chained mode. + * + * Determines if the controller is operating in chained mode. In this mode, only internal + * interfaces are available, and all subscribers are expected to be disabled. This prevents + * concurrent writing to the controller's inputs from multiple sources. + * + * @return `true` if the controller is in chained mode, `false` otherwise. + */ + virtual bool is_in_chained_mode() const = 0; /** @@ -308,10 +335,14 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::string get_name() const; - /// Enable or disable introspection of the controller. - /** - * \param[in] enable Enable introspection if true, disable otherwise. - */ + /** + * @brief Enable or disable introspection of the controller. + * + * Controls whether introspection is enabled for the controller. + * + * @param[in] enable Set to `true` to enable introspection, `false` to disable it. + */ + void enable_introspection(bool enable); protected: diff --git a/controller_interface/include/controller_interface/helpers.hpp b/controller_interface/include/controller_interface/helpers.hpp index dfd29841bf..5e573fe2df 100644 --- a/controller_interface/include/controller_interface/helpers.hpp +++ b/controller_interface/include/controller_interface/helpers.hpp @@ -21,21 +21,23 @@ namespace controller_interface { -/// Reorder interfaces with references according to joint names or full interface names. /** - * Method to reorder and check if all expected interfaces are provided for the joint. - * Fill `ordered_interfaces` with references from `unordered_interfaces` in the same order as in - * `ordered_names`. + * @brief Reorder interfaces with references according to joint names or full interface names. * - * \param[in] unordered_interfaces vector with loaned unordered state or command interfaces. - * \param[in] ordered_names vector with ordered names to order \p unordered_interfaces. - * The valued inputs are list of joint names or interface full names. - * If joint names are used for ordering, \p interface_type specifies valid interface. - * If full interface names are used for ordering, \p interface_type should be empty string (""). - * \param[in] interface_type used for ordering interfaces with respect to joint names. - * \param[out] ordered_interfaces vector with ordered interfaces. - * \return true if all interfaces or joints in \p ordered_names are found, otherwise false. + * Reorders and verifies that all expected interfaces are provided for the specified joints. + * Populates `ordered_interfaces` with references from `unordered_interfaces` in the same + * order as specified in `ordered_names`. + * + * @param[in] unordered_interfaces Vector of loaned unordered state or command interfaces. + * @param[in] ordered_names Vector of ordered names used to sort `unordered_interfaces`. + * The values can be either a list of joint names or full interface names. + * If joint names are used, `interface_type` must be specified. + * If full interface names are used, `interface_type` should be an empty string (`""`). + * @param[in] interface_type Specifies the interface type when ordering by joint names. + * @param[out] ordered_interfaces Vector to store the ordered interfaces. + * @return `true` if all interfaces or joints in `ordered_names` are found, otherwise `false`. */ + template bool get_ordered_interfaces( std::vector & unordered_interfaces, const std::vector & ordered_names, diff --git a/controller_interface/include/semantic_components/force_torque_sensor.hpp b/controller_interface/include/semantic_components/force_torque_sensor.hpp index 79d182397e..9a581d9a77 100644 --- a/controller_interface/include/semantic_components/force_torque_sensor.hpp +++ b/controller_interface/include/semantic_components/force_torque_sensor.hpp @@ -45,15 +45,22 @@ class ForceTorqueSensor : public SemanticComponentInterface::quiet_NaN()); } - /// Constructor for 6D FTS with custom interface names. /** - * Constructor for 6D FTS with custom interface names or FTS with less then six measurement axes, - * e.g., 1D and 2D force load cells. - * For non existing axes interface is empty string, i.e., (""); - * - * The name should be in the following order: - * force X, force Y, force Z, torque X, torque Y, torque Z. - */ + * @brief Constructor for a 6D Force-Torque Sensor (FTS) with custom interface names. + * + * Initializes a 6D FTS with custom interface names or an FTS with fewer than six + * measurement axes (e.g., 1D or 2D force load cells). + * For non-existing axes, the corresponding interface should be an empty string (`""`). + * + * The names should be provided in the following order: + * - Force X + * - Force Y + * - Force Z + * - Torque X + * - Torque Y + * - Torque Z + */ + ForceTorqueSensor( const std::string & interface_force_x, const std::string & interface_force_y, const std::string & interface_force_z, const std::string & interface_torque_x, @@ -83,12 +90,14 @@ class ForceTorqueSensor : public SemanticComponentInterface get_forces() const { update_data_from_interfaces(); @@ -97,12 +106,14 @@ class ForceTorqueSensor : public SemanticComponentInterface get_torques() const { update_data_from_interfaces(); @@ -111,14 +122,21 @@ class ForceTorqueSensor : public SemanticComponentInterface {name + "/" + "linear_acceleration.z"}}) { } - /// Return orientation. /** - * Return orientation reported by an IMU - * - * \return Array of size 4 with orientation quaternion (x,y,z,w). - */ + * @brief Retrieve the orientation reported by an IMU. + * + * Returns the orientation as a quaternion (x, y, z, w). + * + * @return An array of size 4 containing the orientation quaternion in the order: [X, Y, Z, W]. + */ + std::array get_orientation() const { update_data_from_interfaces(); @@ -56,12 +58,14 @@ class IMUSensor : public SemanticComponentInterface return orientation; } - /// Return angular velocity. /** - * Return angular velocity reported by an IMU - * - * \return array of size 3 with angular velocity values (x, y, z). - */ + * @brief Retrieve the angular velocity reported by an IMU. + * + * Returns the angular velocity measured along the X, Y, and Z axes. + * + * @return An array of size 3 containing angular velocity values in the order: [X, Y, Z]. + */ + std::array get_angular_velocity() const { update_data_from_interfaces(); @@ -70,12 +74,14 @@ class IMUSensor : public SemanticComponentInterface return angular_velocity; } - /// Return linear acceleration. /** - * Return linear acceleration reported by an IMU - * - * \return array of size 3 with linear acceleration values (x, y, z). - */ + * @brief Retrieve the linear acceleration reported by an IMU. + * + * Returns the linear acceleration measured along the X, Y, and Z axes. + * + * @return An array of size 3 containing linear acceleration values in the order: [X, Y, Z]. + */ + std::array get_linear_acceleration() const { update_data_from_interfaces(); @@ -84,11 +90,15 @@ class IMUSensor : public SemanticComponentInterface return linear_acceleration; } - /// Return Imu message with orientation, angular velocity and linear acceleration /** - * Constructs and return a IMU message from the current values. - * \return imu message from values; - */ + * @brief Construct and return an IMU message with orientation, angular velocity, and linear acceleration. + * + * Creates an IMU message using the current sensor values, including orientation, angular velocity, + * and linear acceleration. + * + * @return An IMU message containing the current orientation, angular velocity, and linear acceleration values. + */ + bool get_values_as_message(sensor_msgs::msg::Imu & message) const { update_data_from_interfaces(); diff --git a/controller_interface/include/semantic_components/led_rgb_device.hpp b/controller_interface/include/semantic_components/led_rgb_device.hpp index 0ecbd1e93d..c48d5946a1 100644 --- a/controller_interface/include/semantic_components/led_rgb_device.hpp +++ b/controller_interface/include/semantic_components/led_rgb_device.hpp @@ -44,18 +44,18 @@ class LedRgbDevice : public SemanticComponentCommandInterface {name + '/' + "orientation.w"}}) { } - /// Update and return position. - /*! - * Update and return current pose position from state interfaces. - * - * \return Array of position coordinates. - */ + /** + * @brief Update and retrieve the current position. + * + * Updates the current pose position using state interfaces and returns the position coordinates. + * + * @return An array containing the current position coordinates. + */ + std::array get_position() const { update_data_from_interfaces(); @@ -54,12 +56,14 @@ class PoseSensor : public SemanticComponentInterface return position; } - /// Update and return orientation - /*! - * Update and return current pose orientation from state interfaces. - * - * \return Array of orientation coordinates in xyzw convention. - */ + /** + * @brief Update and retrieve the current orientation. + * + * Updates the current pose orientation using state interfaces and returns the orientation coordinates. + * + * @return An array containing the orientation coordinates in XYZW convention. + */ + std::array get_orientation() const { update_data_from_interfaces(); @@ -68,10 +72,12 @@ class PoseSensor : public SemanticComponentInterface return orientation; } - /// Fill pose message with current values. /** - * Fill a pose message with current position and orientation from the state interfaces. - */ + * @brief Populate a pose message with the current values. + * + * Fills a pose message using the current position and orientation obtained from state interfaces. + */ + bool get_values_as_message(geometry_msgs::msg::Pose & message) const { update_data_from_interfaces(); diff --git a/controller_interface/include/semantic_components/range_sensor.hpp b/controller_interface/include/semantic_components/range_sensor.hpp index 2a9d2bb1e2..fe1817c410 100644 --- a/controller_interface/include/semantic_components/range_sensor.hpp +++ b/controller_interface/include/semantic_components/range_sensor.hpp @@ -46,11 +46,14 @@ class RangeSensor : public SemanticComponentInterface return std::numeric_limits::quiet_NaN(); } - /// Return Range message with range in meters /** - * Constructs and return a Range message from the current values. - * \return Range message from values; - */ + * @brief Construct and return a Range message with the current range in meters. + * + * Creates a Range message using the current sensor values. + * + * @return A Range message containing the current range measurement in meters. + */ + bool get_values_as_message(sensor_msgs::msg::Range & message) const { message.range = get_range(); diff --git a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp index 5569b7ba9d..6529905ce5 100644 --- a/controller_interface/include/semantic_components/semantic_component_command_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_command_interface.hpp @@ -37,12 +37,14 @@ class SemanticComponentCommandInterface virtual ~SemanticComponentCommandInterface() = default; - /// Assign loaned command interfaces from the hardware. /** - * Assign loaned command interfaces on the controller start. - * - * \param[in] command_interfaces vector of command interfaces provided by the controller. - */ + * @brief Assign loaned command interfaces from the hardware. + * + * Assigns the provided loaned command interfaces when the controller starts. + * + * @param[in] command_interfaces A vector of command interfaces provided to the controller. + */ + bool assign_loaned_command_interfaces( std::vector & command_interfaces) { @@ -53,22 +55,28 @@ class SemanticComponentCommandInterface /// Release loaned command interfaces from the hardware. void release_interfaces() { command_interfaces_.clear(); } - /// Definition of command interface names for the component. /** - * The function should be used in "command_interface_configuration()" of a controller to provide - * standardized command interface names semantic component. - * - * \default Default implementation defined command interfaces as "name/NR" where NR is number - * from 0 to size of values; - * \return list of strings with command interface names for the semantic component. - */ + * @brief Define command interface names for the component. + * + * This function should be used in `command_interface_configuration()` of a controller + * to provide standardized command interface names for a semantic component. + * + * @note The default implementation defines command interfaces as `"name/NR"`, + * where `NR` is a number from 0 to the size of the values. + * + * @return A list of strings containing the command interface names for the semantic component. + */ + const std::vector & get_command_interface_names() const { return interface_names_; } - /// Return all values. /** - * \return true if it gets all the values, else false (i.e., invalid size or if the method - * ``hardware_interface::LoanedCommandInterface::set_value`` fails). - */ + * @brief Retrieve all values. + * + * @return true if all values are successfully retrieved, false otherwise + * (e.g., due to invalid size or failure of + * `hardware_interface::LoanedCommandInterface::set_value`). + */ + bool set_values(const std::vector & values) { // check we have sufficient memory @@ -85,10 +93,12 @@ class SemanticComponentCommandInterface return all_set; } - /// Set values from MessageInputType - /** - * \return True if all values were set successfully, false otherwise. - */ + /** + * @brief Set values from MessageInputType. + * + * @return true if all values were set successfully, false otherwise. + */ + virtual bool set_values_from_message(const MessageInputType & /* message */) = 0; protected: diff --git a/controller_interface/include/semantic_components/semantic_component_interface.hpp b/controller_interface/include/semantic_components/semantic_component_interface.hpp index 76a34aaddd..99294c7c28 100644 --- a/controller_interface/include/semantic_components/semantic_component_interface.hpp +++ b/controller_interface/include/semantic_components/semantic_component_interface.hpp @@ -42,12 +42,14 @@ class SemanticComponentInterface virtual ~SemanticComponentInterface() = default; - /// Assign loaned state interfaces from the hardware. - /** - * Assign loaned state interfaces on the controller start. - * - * \param[in] state_interfaces vector of interfaces provided by the controller. - */ + /** + * @brief Assign loaned state interfaces from the hardware. + * + * This function assigns loaned state interfaces when the controller starts. + * + * @param[in] state_interfaces A vector of state interfaces provided by the controller. + */ + bool assign_loaned_state_interfaces( std::vector & state_interfaces) { @@ -58,15 +60,18 @@ class SemanticComponentInterface /// Release loaned interfaces from the hardware. void release_interfaces() { state_interfaces_.clear(); } - /// Definition of state interface names for the component. - /** - * The function should be used in "state_interface_configuration()" of a controller to provide - * standardized interface names semantic component. - * - * \default Default implementation defined state interfaces as "name/NR" where NR is number - * from 0 to size of values; - * \return list of strings with state interface names for the semantic component. - */ + /** + * @brief Define state interface names for the component. + * + * This function should be used in `state_interface_configuration()` of a controller + * to provide standardized state interface names for the semantic component. + * + * @note The default implementation defines state interfaces as `"name/NR"`, + * where `NR` is a number from 0 to the size of the values. + * + * @return A list of strings containing state interface names for the semantic component. + */ + virtual std::vector get_state_interface_names() { if (interface_names_.empty()) @@ -79,10 +84,12 @@ class SemanticComponentInterface return interface_names_; } - /// Return all values. - /** - * \return true if it gets all the values, else false - */ + /** + * @brief Retrieve all values. + * + * @return `true` if all values are successfully retrieved, `false` otherwise. + */ + bool get_values(std::vector & values) const { // check we have sufficient memory @@ -98,10 +105,12 @@ class SemanticComponentInterface return true; } - /// Return values as MessageReturnType /** - * \return false by default - */ + * @brief Return values as a MessageReturnType. + * + * @return `false` by default. + */ + bool get_values_as_message(MessageReturnType & /* message */) { return false; } protected: diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index ec17bec402..fdc2f59b05 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -82,10 +82,12 @@ class ControllerManager : public rclcpp::Node virtual ~ControllerManager(); - /// Shutdown all controllers in the controller manager. /** - * \return true if all controllers are successfully shutdown, false otherwise. - */ + * @brief Shutdown all controllers in the controller manager. + * + * @return `true` if all controllers are successfully shut down, `false` otherwise. + */ + bool shutdown_controllers(); void robot_description_callback(const std_msgs::msg::String & msg); @@ -95,12 +97,16 @@ class ControllerManager : public rclcpp::Node controller_interface::ControllerInterfaceBaseSharedPtr load_controller( const std::string & controller_name, const std::string & controller_type); - /// load_controller loads a controller by name, the type must be defined in the parameter server. - /** - * \param[in] controller_name as a string. - * \return controller - * \see Documentation in controller_manager_msgs/LoadController.srv - */ + /** + * @brief Load a controller by name. + * + * The controller type must be defined in the parameter server. + * + * @param[in] controller_name Name of the controller as a string. + * @return Loaded controller. + * @see Documentation in controller_manager_msgs/LoadController.srv + */ + controller_interface::ControllerInterfaceBaseSharedPtr load_controller( const std::string & controller_name); @@ -130,84 +136,107 @@ class ControllerManager : public rclcpp::Node return add_controller_impl(controller_spec); } - /// configure_controller Configure controller by name calling their "configure" method. /** - * \param[in] controller_name as a string. - * \return configure controller response - * \see Documentation in controller_manager_msgs/ConfigureController.srv - */ + * @brief Configure a controller by name. + * + * Calls the "configure" method of the specified controller. + * + * @param[in] controller_name Name of the controller as a string. + * @return Response of the configure controller operation. + * @see Documentation in controller_manager_msgs/ConfigureController.srv + */ + controller_interface::return_type configure_controller(const std::string & controller_name); - /// switch_controller Deactivates some controllers and activates others. - /** - * \param[in] activate_controllers is a list of controllers to activate. - * \param[in] deactivate_controllers is a list of controllers to deactivate. - * \param[in] set level of strictness (BEST_EFFORT or STRICT) - * \see Documentation in controller_manager_msgs/SwitchController.srv - */ + /** + * @brief Switch controllers by deactivating some and activating others. + * + * This method deactivates a list of specified controllers and activates another list of controllers. + * + * @param[in] activate_controllers List of controllers to activate. + * @param[in] deactivate_controllers List of controllers to deactivate. + * @param[in] strictness Level of strictness (BEST_EFFORT or STRICT). + * @see Documentation in controller_manager_msgs/SwitchController.srv + */ + controller_interface::return_type switch_controller( const std::vector & activate_controllers, const std::vector & deactivate_controllers, int strictness, bool activate_asap = kWaitForAllResources, const rclcpp::Duration & timeout = rclcpp::Duration::from_nanoseconds(kInfiniteTimeout)); - /// Read values to state interfaces. - /** - * Read current values from hardware to state interfaces. - * **The method called in the (real-time) control loop.** - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured period of the last control loop iteration - */ +/** + * @brief Read current values from hardware to state interfaces. + * + * This method is called during the real-time control loop to update state interfaces with + * the latest values from the hardware. + * + * @param[in] time The time at the start of the current control loop iteration. + * @param[in] period The measured duration of the last control loop iteration. + */ + void read(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Run update on controllers - /** - * Call update of all controllers. - * **The method called in the (real-time) control loop.** - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured period of the last control loop iteration - */ + /** + * @brief Execute update on all controllers. + * + * Calls the update method for all controllers during the real-time control loop. + * + * @param[in] time The time at the start of the current control loop iteration. + * @param[in] period The measured duration of the last control loop iteration. + */ + controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period); - /// Write values from command interfaces. /** - * Write values from command interface into hardware. - * **The method called in the (real-time) control loop.** - * - * \param[in] time The time at the start of this control loop iteration - * \param[in] period The measured period of the last control loop iteration - */ + * @brief Write values from command interfaces to hardware. + * + * Transfers values from the command interfaces to the hardware during the real-time control loop. + * + * @param[in] time The time at the start of the current control loop iteration. + * @param[in] period The measured duration of the last control loop iteration. + */ + void write(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Deterministic (real-time safe) callback group, e.g., update function. - /** - * Deterministic (real-time safe) callback group for the update function. Default behavior - * is read hardware, update controller and finally write new values to the hardware. - */ + /** + * @brief Deterministic (real-time safe) callback group for the update function. + * + * This callback group ensures real-time safe execution for the update function. The default + * behavior follows a structured sequence: + * 1. Read values from the hardware. + * 2. Update the controller. + * 3. Write new values back to the hardware. + */ + // TODO(anyone): Due to issues with the MutliThreadedExecutor, this control loop does not rely on // the executor (see issue #260). // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_; - /// Interface for external components to check if Resource Manager is initialized. - /** - * Checks if components in Resource Manager are loaded and initialized. - * \returns true if they are initialized, false otherwise. - */ + /** + * @brief Checks if the Resource Manager is initialized. + * + * This method allows external components to verify whether all components + * in the Resource Manager have been successfully loaded and initialized. + * + * @return true if the Resource Manager is initialized, false otherwise. + */ + bool is_resource_manager_initialized() const { return resource_manager_ && resource_manager_->are_components_initialized(); } - /// Update rate of the main control loop in the controller manager. - /** - * Update rate of the main control loop in the controller manager. - * The method is used for per-controller update rate support. - * - * \returns update rate of the controller manager. - */ + /** + * @brief Gets the update rate of the main control loop in the controller manager. + * + * This method retrieves the update rate of the main control loop, which is + * used to support per-controller update rates. + * + * @return The update rate of the controller manager. + */ + unsigned int get_update_rate() const; protected: @@ -218,52 +247,64 @@ class ControllerManager : public rclcpp::Node void manage_switch(); - /// Deactivate chosen controllers from real-time controller list. /** - * Deactivate controllers with names \p controllers_to_deactivate from list \p rt_controller_list. - * The controller list will be iterated as many times as there are controller names. - * - * \param[in] rt_controller_list controllers in the real-time list. - * \param[in] controllers_to_deactivate names of the controller that have to be deactivated. - */ + * @brief Deactivates selected controllers from the real-time controller list. + * + * This method deactivates controllers specified in \p controllers_to_deactivate + * from the given \p rt_controller_list. The controller list will be iterated as + * many times as there are controller names. + * + * @param[in] rt_controller_list List of controllers in the real-time system. + * @param[in] controllers_to_deactivate Names of controllers to be deactivated. + */ + void deactivate_controllers( const std::vector & rt_controller_list, const std::vector controllers_to_deactivate); - /** - * Switch chained mode for all the controllers with respect to the following cases: - * - a preceding controller is getting activated --> switch controller to chained mode; - * - all preceding controllers are deactivated --> switch controller from chained mode. - * - * \param[in] chained_mode_switch_list list of controller to switch chained mode. - * \param[in] to_chained_mode flag if controller should be switched *to* or *from* chained mode. - */ + /** + * @brief Switches chained mode for all controllers based on activation state. + * + * This method updates the chained mode of controllers according to the following conditions: + * - A preceding controller is being activated → switch the controller *to* chained mode. + * - All preceding controllers are deactivated → switch the controller *from* chained mode. + * + * @param[in] chained_mode_switch_list List of controllers to switch chained mode. + * @param[in] to_chained_mode Flag indicating whether to switch *to* (`true`) or *from* (`false`) chained mode. + */ + void switch_chained_mode( const std::vector & chained_mode_switch_list, bool to_chained_mode); - /// Activate chosen controllers from real-time controller list. /** - * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. - * The controller list will be iterated as many times as there are controller names. - * - * \param[in] rt_controller_list controllers in the real-time list. - * \param[in] controllers_to_activate names of the controller that have to be activated. - */ + * @brief Activates selected controllers from the real-time controller list. + * + * This method activates controllers specified in \p controllers_to_activate from the + * given \p rt_controller_list. The controller list will be iterated as many times as + * there are controller names to ensure proper activation. + * + * @param[in] rt_controller_list List of controllers in the real-time system. + * @param[in] controllers_to_activate Names of the controllers that should be activated. + */ + void activate_controllers( const std::vector & rt_controller_list, const std::vector controllers_to_activate); - /// Activate chosen controllers from real-time controller list. - /** - * Activate controllers with names \p controllers_to_activate from list \p rt_controller_list. - * The controller list will be iterated as many times as there are controller names. - * - * *NOTE*: There is currently not difference to `activate_controllers` method. - * Check https://github.com/ros-controls/ros2_control/issues/263 for more information. - * - * \param[in] rt_controller_list controllers in the real-time list. - * \param[in] controllers_to_activate names of the controller that have to be activated. - */ + /** + * @brief Activates the specified controllers from the real-time controller list. + * + * This method activates controllers listed in \p controllers_to_activate from the provided + * \p rt_controller_list. The controller list is iterated as many times as there are + * controller names to ensure activation. + * + * @note Currently, this method behaves identically to `activate_controllers`. + * See for details. + * + * @param[in] rt_controller_list List of controllers in the real-time system. + * @param[in] controllers_to_activate Names of the controllers to activate. + */ + void activate_controllers_asap( const std::vector & rt_controller_list, const std::vector controllers_to_activate); @@ -344,66 +385,76 @@ class ControllerManager : public rclcpp::Node */ void propagate_deactivation_of_chained_mode(const std::vector & controllers); - /// Check if all the following controllers will be in active state and in the chained mode - /// after controllers' switch. /** - * Check recursively that all following controllers of the @controller_it - * - are already active, - * - will not be deactivated, - * - or will be activated. - * The following controllers are added to the request to switch in the chained mode or removed - * from the request to switch from the chained mode. - * - * For each controller the whole chain of following controllers is checked. - * - * NOTE: The automatically adding of following controller into activate list is not implemented - * yet. - * - * \param[in] controllers list with controllers. - * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all following - * controllers will be automatically added to the activate request list if they are not in the - * deactivate request. - * \param[in] controller_it iterator to the controller for which the following controllers are - * checked. - * - * \returns return_type::OK if all following controllers pass the checks, otherwise - * return_type::ERROR. - */ + * @brief Checks the activation state and chained mode of subsequent controllers. + * + * Validates that all controllers following \p controller_it: + * - Are already active, + * - Will not be deactivated, + * - Or will be activated. + * + * If necessary, subsequent controllers are added to or removed from the request + * to switch in/out of chained mode. The entire chain of controllers is verified. + * + * @note Automatic addition of subsequent controllers to the activation list + * is not yet implemented. + * + * @param[in] controllers List of controllers. + * @param[in] strictness If `"MANIPULATE_CONTROLLERS_CHAIN"`, subsequent controllers + * are automatically added to the activation request unless + * already in the deactivation request. + * @param[in] controller_it Iterator to the controller whose subsequent controllers + * are being checked. + * + * @return `return_type::OK` if all checks pass, otherwise `return_type::ERROR`. + */ + + controller_interface::return_type check_following_controllers_for_activate( const std::vector & controllers, int strictness, const ControllersListIterator controller_it); - /// Check if all the preceding controllers will be in inactive state after controllers' switch. /** - * Check that all preceding controllers of the @controller_it - * - are inactive, - * - will be deactivated, - * - and will not be activated. - * - * NOTE: The automatically adding of preceding controllers into deactivate list is not implemented - * yet. - * - * \param[in] controllers list with controllers. - * \param[in] strictness if value is equal "MANIPULATE_CONTROLLERS_CHAIN" then all preceding - * controllers will be automatically added to the deactivate request list. - * \param[in] controller_it iterator to the controller for which the preceding controllers are - * checked. - * - * \returns return_type::OK if all preceding controllers pass the checks, otherwise - * return_type::ERROR. - */ + * @brief Validate that all preceding controllers will be inactive after a controllers' switch. + * + * This method checks whether all controllers preceding \p controller_it: + * - Are currently inactive, + * - Will be deactivated, + * - And will not be activated. + * + * If any of these conditions are not met, the validation fails. + * + * @note Automatic addition of preceding controllers to the deactivation request list + * is not yet implemented. + * + * @param[in] controllers List of controllers. + * @param[in] strictness If set to `"MANIPULATE_CONTROLLERS_CHAIN"`, all preceding controllers + * will be automatically added to the deactivation request list. + * @param[in] controller_it Iterator pointing to the controller whose preceding controllers + * are being checked. + * + * @return `return_type::OK` if all preceding controllers pass the checks, otherwise + * `return_type::ERROR`. + */ + controller_interface::return_type check_preceeding_controllers_for_deactivate( const std::vector & controllers, int strictness, const ControllersListIterator controller_it); /// Checks if the fallback controllers of the given controllers are in the right - /// state, so they can be activated immediately /** - * \param[in] controllers is a list of controllers to activate. - * \param[in] controller_it is the iterator pointing to the controller to be activated. - * \return return_type::OK if all fallback controllers are in the right state, otherwise - * return_type::ERROR. - */ + * @brief Ensure that fallback controllers are in the correct state for immediate activation. + * + * This function checks whether all fallback controllers are in a state that allows + * them to be activated immediately. + * + * @param[in] controllers List of controllers to activate. + * @param[in] controller_it Iterator pointing to the specific controller to be activated. + * + * @return `return_type::OK` if all fallback controllers are in the correct state, + * otherwise `return_type::ERROR`. + */ + controller_interface::return_type check_fallback_controllers_state_pre_activation( const std::vector & controllers, const ControllersListIterator controller_it); @@ -462,11 +513,14 @@ class ControllerManager : public rclcpp::Node std::shared_ptr> chainable_loader_; - /// Best effort (non real-time safe) callback group, e.g., service callbacks. /** - * Best effort (non real-time safe) callback group for callbacks that can possibly break - * real-time requirements, for example, service callbacks. - */ + * @brief Best-effort (non real-time safe) callback group. + * + * This callback group is intended for operations that do not meet real-time + * constraints. It is suitable for tasks that may introduce delays or + * unpredictability, such as service callbacks. + */ + rclcpp::CallbackGroup::SharedPtr best_effort_callback_group_; /** @@ -485,12 +539,17 @@ class ControllerManager : public rclcpp::Node // *INDENT-OFF* public: // *INDENT-ON* - /// update_and_get_used_by_rt_list Makes the "updated" list the "used by rt" list /** - * \warning Should only be called by the RT thread, no one should modify the - * updated list while it's being used - * \return reference to the updated list - */ + * @brief Updates and retrieves the real-time usage list. + * + * This function replaces the "used by RT" list with the "updated" list. + * + * @warning This function should only be called by the real-time (RT) thread. + * Modifying the updated list while it is being used may lead to unpredictable behavior. + * + * @return Reference to the updated list. + */ + std::vector & update_and_get_used_by_rt_list(); /** @@ -504,12 +563,14 @@ class ControllerManager : public rclcpp::Node std::vector & get_unused_list( const std::lock_guard & guard); - /// get_updated_list Returns a const reference to the most updated list. /** - * \warning May or may not being used by the realtime thread, read-only reference for safety - * \param[in] guard Guard needed to make sure the caller is the only one accessing the unused by - * rt list - */ + * @brief Retrieves a constant reference to the most recently updated list. + * + * This function returns a read-only reference to the latest updated list. + * + * @warning The list may or may not be currently used by the real-time (RT) thread. + * Ensure that it is only accesse + const std::vector & get_updated_list( const std::lock_guard & guard) const; diff --git a/controller_manager/include/controller_manager/controller_spec.hpp b/controller_manager/include/controller_manager/controller_spec.hpp index 9cc508772d..51b92b3847 100644 --- a/controller_manager/include/controller_manager/controller_spec.hpp +++ b/controller_manager/include/controller_manager/controller_spec.hpp @@ -31,12 +31,13 @@ namespace controller_manager using MovingAverageStatistics = libstatistics_collector::moving_average_statistics::MovingAverageStatistics; -/// Controller Specification /** - * This struct contains both a pointer to a given controller, \ref c, as well - * as information about the controller, \ref info. + * @brief Controller specification. * + * This struct contains both a pointer to a given controller (`c`) and + * information about the controller (`info`). */ + struct ControllerSpec { ControllerSpec() diff --git a/hardware_interface/include/hardware_interface/component_parser.hpp b/hardware_interface/include/hardware_interface/component_parser.hpp index 9de1813577..988ecab323 100644 --- a/hardware_interface/include/hardware_interface/component_parser.hpp +++ b/hardware_interface/include/hardware_interface/component_parser.hpp @@ -23,12 +23,16 @@ namespace hardware_interface { -/// Search XML snippet from URDF for information about a control component. /** - * \param[in] urdf string with robot's URDF - * \return vector filled with information about robot's control resources - * \throws std::runtime_error if a robot attribute or tag is not found + * @brief Searches an XML snippet from URDF for control component information. + * + * Extracts control resource details from the given URDF string. + * + * @param[in] urdf String containing the robot's URDF. + * @return A vector filled with information about the robot's control resources. + * @throws std::runtime_error If a required robot attribute or tag is not found. */ + std::vector parse_control_resources_from_urdf(const std::string & urdf); /** diff --git a/hardware_interface/include/hardware_interface/controller_info.hpp b/hardware_interface/include/hardware_interface/controller_info.hpp index d814ca7ae2..be84f84b5d 100644 --- a/hardware_interface/include/hardware_interface/controller_info.hpp +++ b/hardware_interface/include/hardware_interface/controller_info.hpp @@ -23,8 +23,12 @@ namespace hardware_interface { /// Controller Information /** - * This struct contains information about a given controller. + * @brief Holds details about a specific controller. + * + * This struct stores essential information related to a controller, + * including its configuration, status, and relevant parameters. */ + struct ControllerInfo { /// Controller name. diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 58e8d86d32..e6a0132b13 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -293,12 +293,14 @@ class CommandInterface : public Handle : Handle(interface_description) { } - /// CommandInterface copy constructor is actively deleted. - /** - * Command interfaces are having a unique ownership and thus - * can't be copied in order to avoid simultaneous writes to - * the same resource. - */ + /// CommandInterface copy constructor is deleted. +/** + * @brief Prevents copying to ensure unique ownership. + * + * Command interfaces have unique ownership to prevent simultaneous writes + * to the same resource. Therefore, copying is explicitly disabled. + */ + CommandInterface(const CommandInterface & other) = delete; CommandInterface(CommandInterface && other) = default; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index d5d25044dd..5259e6792c 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -53,19 +53,18 @@ class ResourceManager rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_interface); /// Constructor for the Resource Manager. - /** - * The implementation loads the specified urdf and initializes the - * hardware components listed within as well as populate their respective - * state and command interfaces. - * - * \param[in] urdf string containing the URDF. - * \param[in] activate_all boolean argument indicating if all resources should be immediately - * activated. Currently used only in tests. - * \param[in] update_rate Update rate of the controller manager to calculate calling frequency - * of async components. - * \param[in] clock_interface reference to the clock interface of the CM node for getting time - * used for triggering async components. - */ +/** + * @brief Initializes hardware components and their interfaces from a URDF. + * + * This constructor loads the provided URDF, initializes listed hardware components, + * and populates their respective state and command interfaces. + * + * @param[in] urdf String containing the URDF. + * @param[in] activate_all If true, activates all resources immediately (used in tests). + * @param[in] update_rate Update rate of the controller manager, determining async component frequency. + * @param[in] clock_interface Reference to the CM node's clock interface for time-based async triggers. + */ + explicit ResourceManager( const std::string & urdf, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, @@ -76,405 +75,496 @@ class ResourceManager virtual ~ResourceManager(); - /// Shutdown all hardware components, irrespective of their state. - /** - * The method is called when the controller manager is being shutdown. - * @return true if all hardware components are successfully shutdown, false otherwise. - */ + /// Shutdown all hardware components, regardless of their state. +/** + * @brief Safely shuts down all hardware components. + * + * This method is invoked when the controller manager is shutting down. + * + * @return True if all hardware components are successfully shut down, false otherwise. + */ + bool shutdown_components(); - /// Load resources from on a given URDF. - /** - * The resource manager can be post-initialized with a given URDF. - * This is mainly used in conjunction with the default constructor - * in which the URDF might not be present at first initialization. - * - * \param[in] urdf string containing the URDF. - * \param[in] update_rate update rate of the main control loop, i.e., of the controller manager. - * \returns false if URDF validation has failed. - */ + /// Load resources from a given URDF. +/** + * @brief Initializes the resource manager with a URDF. + * + * This method allows post-initialization of the resource manager when the URDF + * is not available during the first initialization. + * + * @param[in] urdf String containing the URDF. + * @param[in] update_rate Update rate of the main control loop (controller manager). + * @return False if URDF validation fails. + */ + virtual bool load_and_initialize_components( const std::string & urdf, const unsigned int update_rate = 100); /** - * @brief if the resource manager load_and_initialize_components(...) function has been called - * this returns true. We want to permit to loading the urdf later on, but we currently don't want - * to permit multiple calls to load_and_initialize_components (reloading/loading different urdf). - * - * @return true if the resource manager has successfully loaded and initialized the components - * @return false if the resource manager doesn't have any components loaded and initialized. - */ + * @brief Checks if the resource manager has loaded and initialized components. + * + * Ensures that `load_and_initialize_components(...)` is only called once. + * While loading the URDF later is permitted, multiple calls to this function + * (e.g., for reloading or loading a different URDF) are not allowed. + * + * @return True if components are successfully loaded and initialized. + * @return False if no components are loaded or initialized. + */ + bool are_components_initialized() const; - /// Claim a state interface given its key. - /** - * The resource is claimed as long as being in scope. - * Once the resource is going out of scope, the destructor - * returns. - * - * \param[in] key String identifier which state interface to claim - * \return state interface - */ + /** + * @brief Claims a state interface using its key. + * + * The resource remains claimed while in scope. Once it goes out of scope, + * it is automatically released by the destructor. + * + * @param[in] key String identifier of the state interface to claim. + * @return The claimed state interface. + */ + LoanedStateInterface claim_state_interface(const std::string & key); - /// Returns all registered state interfaces keys. - /** - * The keys are collected from each loaded hardware component. - * \return Vector of strings, containing all registered keys. - */ + /** + * @brief Retrieves all registered state interface keys. + * + * The keys are gathered from all loaded hardware components. + * + * @return A vector of strings containing all registered state interface keys. + */ + std::vector state_interface_keys() const; - /// Returns all available state interfaces keys. /** - * The keys are collected from the available list. - * \return Vector of strings, containing all available state interface names. - */ + * @brief Retrieves all available state interface keys. + * + * The keys are gathered from the available list. + * + * @return A vector of strings containing all available + std::vector available_state_interfaces() const; - /// Checks whether a state interface is available under the given key. /** - * \return true if interface is available, false otherwise. - */ + * @brief Checks if a state interface is available for the given key. + * + * @param[in] key String identifier of the state interface. + * @return true if the interface is available, false otherwise. + */ + bool state_interface_is_available(const std::string & name) const; - /// Add controllers' exported state interfaces to resource manager. - /** - * Interface for transferring management of exported state interfaces to resource manager. - * When chaining controllers, state interfaces are used by the preceding - * controllers. - * Therefore, they should be managed in the same way as state interface of hardware. - * - * \param[in] controller_name name of the controller which state interfaces are imported. - * \param[in] interfaces list of controller's state interfaces as StateInterfaces. - */ + /** + * @brief Adds controllers' exported state interfaces to the resource manager. + * + * This method transfers management of exported state interfaces to the resource manager. + * When chaining controllers, state interfaces are utilized by preceding controllers and + * should be managed similarly to hardware state interfaces. + * + * @param[in] controller_name Name of the controller whose state interfaces are imported. + * @param[in] interfaces List of the controller's state interfaces as StateInterfaces. + */ + void import_controller_exported_state_interfaces( const std::string & controller_name, std::vector & interfaces); - /// Get list of exported tate interface of a controller. - /** - * Returns lists of stored exported state interfaces names for a controller. - * - * \param[in] controller_name for which list of state interface names is returned. - * \returns list of reference interface names. - */ + /// Get list of exported state interfaces of a controller. +/** + * Retrieves the list of stored exported state interface names for a given controller. + * + * @param[in] controller_name Name of the controller whose state interfaces are requested. + * @return List of reference interface names. + */ + std::vector get_controller_exported_state_interface_names( const std::string & controller_name); - /// Add controller's exported state interfaces to available list. - /** - * Adds state interfacess of a controller with given name to the available list. This method - * should be called when a controller gets activated with chained mode turned on. That means, the - * controller's exported state interfaces can be used by another controllers in chained - * architectures. - * - * \param[in] controller_name name of the controller which interfaces should become available. - */ + /// Add controller's exported state interfaces to the available list. +/** + * Adds the state interfaces of a controller to the available list. + * This method should be called when a controller is activated with chained mode enabled, + * allowing its exported state interfaces to be used by other controllers in a chained architecture. + * + * @param[in] controller_name Name of the controller whose interfaces should become available. + */ + void make_controller_exported_state_interfaces_available(const std::string & controller_name); - /// Remove controller's exported state interface to available list. - /** - * Removes interfaces of a controller with given name from the available list. This method should - * be called when a controller gets deactivated and its reference interfaces cannot be used by - * another controller anymore. - * - * \param[in] controller_name name of the controller which interfaces should become unavailable. - */ + /// Remove controller's exported state interfaces from the available list. +/** + * Removes the state interfaces of a controller from the available list. + * This method should be called when a controller is deactivated, + * ensuring its reference interfaces are no longer accessible to other controllers. + * + * @param[in] controller_name Name of the controller whose interfaces should be removed. + */ + void make_controller_exported_state_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers exported state interfaces from resource manager. - /** - * Remove exported state interfaces from resource manager, i.e., resource storage. - * The interfaces will be deleted from all internal maps and lists. - * - * \param[in] controller_name list of interface names that will be deleted from resource manager. - */ + /// Remove controller's exported state interfaces from the resource manager. +/** + * Removes the exported state interfaces of a controller from the resource manager, + * ensuring they are deleted from all internal maps and lists. + * + * @param[in] controller_name Name of the controller whose interfaces will be removed. + */ + void remove_controller_exported_state_interfaces(const std::string & controller_name); - /// Add controllers' reference interfaces to resource manager. - /** - * Interface for transferring management of reference interfaces to resource manager. - * When chaining controllers, reference interfaces are used as command interface of preceding - * controllers. - * Therefore, they should be managed in the same way as command interface of hardware. - * - * \param[in] controller_name name of the controller which reference interfaces are imported. - * \param[in] interfaces list of controller's reference interfaces as CommandInterfaces. - */ + /// Add controllers' reference interfaces to the resource manager. +/** + * Transfers management of reference interfaces to the resource manager. + * In chained controllers, reference interfaces serve as command interfaces + * for preceding controllers and must be managed similarly to hardware command interfaces. + * + * @param[in] controller_name Name of the controller whose reference interfaces are imported. + * @param[in] interfaces List of the controller's reference interfaces as CommandInterfaces. + */ + void import_controller_reference_interfaces( const std::string & controller_name, const std::vector & interfaces); - /// Get list of reference interface of a controller. - /** - * Returns lists of stored reference interfaces names for a controller. - * - * \param[in] controller_name for which list of reference interface names is returned. - * \returns list of reference interface names. - */ + /// Get list of reference interfaces for a controller. +/** + * Retrieves the stored reference interface names associated with a given controller. + * + * @param[in] controller_name Name of the controller whose reference interfaces are requested. + * @return List of reference interface names. + */ + std::vector get_controller_reference_interface_names( const std::string & controller_name); - /// Add controller's reference interface to available list. - /** - * Adds interfaces of a controller with given name to the available list. This method should be - * called when a controller gets activated with chained mode turned on. That means, the - * controller's reference interfaces can be used by another controller in chained architectures. - * - * \param[in] controller_name name of the controller which interfaces should become available. - */ + /// Add a controller's reference interfaces to the available list. +/** + * Marks the reference interfaces of a specified controller as available. + * This method should be called when a controller is activated in chained mode, + * allowing its reference interfaces to be used by other controllers in a chained setup. + * + * @param[in] controller_name Name of the controller whose reference interfaces should be available. + */ + void make_controller_reference_interfaces_available(const std::string & controller_name); - /// Remove controller's reference interface to available list. - /** - * Removes interfaces of a controller with given name from the available list. This method should - * be called when a controller gets deactivated and its reference interfaces cannot be used by - * another controller anymore. - * - * \param[in] controller_name name of the controller which interfaces should become unavailable. - */ + /// Remove a controller's reference interfaces from the available list. +/** + * This method unregisters the reference interfaces of a specified controller, + * ensuring they are no longer available for use by other controllers. + * It should be called when the controller is deactivated. + * + * @param[in] controller_name Name of the controller whose reference interfaces should be removed. + */ + void make_controller_reference_interfaces_unavailable(const std::string & controller_name); - /// Remove controllers reference interfaces from resource manager. - /** - * Remove reference interfaces from resource manager, i.e., resource storage. - * The interfaces will be deleted from all internal maps and lists. - * - * \param[in] controller_name list of interface names that will be deleted from resource manager. - */ + /// Remove controllers' reference interfaces from the resource manager. +/** + * Deletes the reference interfaces associated with a given controller from the resource manager. + * This ensures that the interfaces are no longer stored or accessible within the system. + * + * @param[in] controller_name Name of the controller whose reference interfaces should be removed. + */ + void remove_controller_reference_interfaces(const std::string & controller_name); - /// Cache mapping between hardware and controllers using it - /** - * Find mapping between controller and hardware based on interfaces controller with - * \p controller_name is using and cache those for later usage. - * - * \param[in] controller_name name of the controller which interfaces are provided. - * \param[in] interfaces list of interfaces controller with \p controller_name is using. - */ +/// Cache the mapping between hardware and controllers using it. +/** + * Identifies and stores the relationship between a controller and the hardware it interacts with. + * This mapping is based on the interfaces used by the controller, allowing for efficient + * access in future operations. + * + * @param[in] controller_name Name of the controller whose interface mappings are being cached. + * @param[in] interfaces List of interfaces used by the specified controller. + */ + void cache_controller_to_hardware( const std::string & controller_name, const std::vector & interfaces); /// Return cached controllers for a specific hardware. - /** - * Return list of cached controller names that use the hardware with name \p hardware_name. - * - * \param[in] hardware_name the name of the hardware for which cached controllers should be - * returned. \returns list of cached controller names that depend on hardware with name \p - * hardware_name. - */ +/** + * Retrieves a list of controller names that have been cached as using the specified hardware. + * This allows for efficient lookup of controllers associated with a given hardware component. + * + * @param[in] hardware_name Name of the hardware for which cached controllers should be retrieved. + * @return List of cached controller names that depend on the specified hardware. + */ + std::vector get_cached_controllers_to_hardware(const std::string & hardware_name); /// Checks whether a command interface is already claimed. - /** - * Any command interface can only be claimed by a single instance. - * \note the equivalent function does not exist for state interfaces. - * These are solely read-only and can thus be used by multiple instances. - * - * \param[in] key string identifying the interface to check. - * \return true if interface is already claimed, false if available. - */ +/** + * Determines if a given command interface is currently claimed. + * A command interface can only be claimed by a single instance at a time. + * + * @note There is no equivalent function for state interfaces since they are read-only + * and can be accessed by multiple instances simultaneously. + * + * @param[in] key String identifier of the interface to check. + * @return true if the interface is already claimed, false if it is available. + */ + bool command_interface_is_claimed(const std::string & key) const; /// Claim a command interface given its key. - /** - * The resource is claimed as long as being in scope. - * Once the resource is going out of scope, the destructor - * returns and thus frees the resource to claimed by others. - * - * \param[in] key String identifier which command interface to claim - * \return command interface - */ +/** + * Claims ownership of a command interface identified by the given key. + * The resource remains claimed as long as it is in scope. + * Once it goes out of scope, the destructor releases it, allowing others to claim it. + * + * @param[in] key String identifier of the command interface to claim. + * @return The claimed command interface. + */ + LoanedCommandInterface claim_command_interface(const std::string & key); - /// Returns all registered command interfaces keys. - /** - * The keys are collected from each loaded hardware component. - * \return vector of strings, containing all registered keys. - */ + /// Returns all registered command interface keys. +/** + * Retrieves a list of all command interface keys registered across the loaded hardware components. + * + * @return A vector of strings containing all registered command interface keys. + */ + std::vector command_interface_keys() const; - /// Returns all available command interfaces keys. - /** - * The keys are collected from the available list. - * \return vector of strings, containing all available command interface names. - */ + /// Returns all available command interface keys. +/** + * Retrieves a list of command interface keys that are currently available for use. + * + * @return A vector of strings containing all available command interface names. + */ + std::vector available_command_interfaces() const; - /// Checks whether a command interface is available under the given name. - /** - * \param[in] name string identifying the interface to check. - * \return true if interface is available, false otherwise. - */ +/// Checks whether a command interface is available under the given name. +/** + * Determines if a command interface with the specified name is currently available. + * + * @param[in] name String identifier of the command interface to check. + * @return true if the interface is available, false otherwise. + */ + bool command_interface_is_available(const std::string & interface) const; - /// Return the number size_t of loaded actuator components. - /** - * \return number of actuator components. - */ + /// Return the number of loaded actuator components. +/** + * Retrieves the total count of actuator components that have been loaded. + * + * @return The number of loaded actuator components as a size_t value. + */ + size_t actuator_components_size() const; /// Return the number of loaded sensor components. - /** - * \return number of sensor components. - */ +/** + * Retrieves the total count of sensor components that have been loaded. + * + * @return The number of loaded sensor components as a size_t value. + */ + size_t sensor_components_size() const; /// Return the number of loaded system components. - /** - * \return size_t number of system components. - */ +/** + * Retrieves the total count of system components that have been loaded. + * + * @return The number of loaded system components as a size_t value. + */ + size_t system_components_size() const; - /// Import a hardware component which is not listed in the URDF - /** - * Components which are initialized outside a URDF can be added post initialization. - * Nevertheless, there should still be `HardwareInfo` available for this component, - * either parsed from a URDF string (easiest) or filled manually. - * - * \note this might invalidate existing state and command interfaces and should thus - * not be called when a controller is running. - * \note given that no hardware_info is available, the component has to be configured - * externally and prior to the call to import. - * \param[in] actuator pointer to the actuator interface. - * \param[in] hardware_info hardware info - */ + /// Import a hardware component which is not listed in the URDF. +/** + * Allows adding hardware components that are initialized outside of the URDF after + * the initial setup. The component must have `HardwareInfo` available, which can be + * either parsed from a URDF string or manually filled. + * + * @note This operation might invalidate existing state and command interfaces and + * should not be called while a controller is running. + * @note If no `hardware_info` is available, the component must be externally configured + * before calling this function. + * + * @param[in] actuator Pointer to the actuator interface. + * @param[in] hardware_info Information about the hardware component. + */ + void import_component( std::unique_ptr actuator, const HardwareInfo & hardware_info); - /// Import a hardware component which is not listed in the URDF - /** - * Components which are initialized outside a URDF can be added post initialization. - * Nevertheless, there should still be `HardwareInfo` available for this component, - * either parsed from a URDF string (easiest) or filled manually. - * - * \note this might invalidate existing state and command interfaces and should thus - * not be called when a controller is running. - * \note given that no hardware_info is available, the component has to be configured - * externally and prior to the call to import. - * \param[in] sensor pointer to the sensor interface. - * \param[in] hardware_info hardware info - */ + /// Import a hardware component which is not listed in the URDF. +/** + * Allows adding hardware sensor components that are initialized outside of the URDF + * after the initial setup. The component must have `HardwareInfo` available, which + * can be either parsed from a URDF string or manually filled. + * + * @note This operation might invalidate existing state and command interfaces and + * should not be called while a controller is running. + * @note If no `hardware_info` is available, the component must be externally configured + * before calling this function. + * + * @param[in] sensor Pointer to the sensor interface. + * @param[in] hardware_info Information about the hardware component. + */ + void import_component( std::unique_ptr sensor, const HardwareInfo & hardware_info); - /// Import a hardware component which is not listed in the URDF - /** - * Components which are initialized outside a URDF can be added post initialization. - * Nevertheless, there should still be `HardwareInfo` available for this component, - * either parsed from a URDF string (easiest) or filled manually. - * - * \note this might invalidate existing state and command interfaces and should thus - * not be called when a controller is running. - * \note given that no hardware_info is available, the component has to be configured - * externally and prior to the call to import. - * \param[in] system pointer to the system interface. - * \param[in] hardware_info hardware info - */ + /// Import a hardware component which is not listed in the URDF. +/** + * Allows adding system hardware components that are initialized outside of the URDF + * after the initial setup. The component must have `HardwareInfo` available, which + * can be either parsed from a URDF string or manually filled. + * + * @note This operation might invalidate existing state and command interfaces and + * should not be called while a controller is running. + * @note If no `hardware_info` is available, the component must be externally configured + * before calling this function. + * + * @param[in] system Pointer to the system interface. + * @param[in] hardware_info Information about the hardware component. + */ + void import_component( std::unique_ptr system, const HardwareInfo & hardware_info); - /// Return status for all components. - /** - * \return map of hardware names and their status. - */ +/// Return status for all components. +/** + * Retrieves the current status of all registered hardware components. + * + * This function provides an overview of all hardware components along with their + * respective statuses. It helps monitor and manage hardware availability and state. + * + * @return A map where the keys are hardware component names and the values represent + * their current status. + */ + + const std::unordered_map & get_components_status(); - /// Prepare the hardware components for a new command interface mode - /** - * Hardware components are asked to prepare a new command interface claim. - * - * \note this is intended for mode-switching when a hardware interface needs to change - * control mode depending on which command interface is claimed. - * \note this is for non-realtime preparing for and accepting new command resource - * combinations. - * \note accept_command_resource_claim is called on all actuators and system components - * and hardware interfaces should return hardware_interface::return_type::OK - * by default - * \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 true if switch can be prepared; false if a component rejects switch request, and if - * at least one of the input interfaces are not existing or not available (i.e., component is not - * in ACTIVE or INACTIVE state). - */ + /// Prepare the hardware components for a new command interface mode. +/** + * Prepares hardware components for a new command interface claim. + * + * This function facilitates the transition of hardware components to a new command interface + * mode. It ensures that the required resources are available and prepares the system for + * mode switching. + * + * @note This is intended for mode-switching when a hardware interface needs + * to change control mode depending on which command interface is claimed. + * @note This is for non-realtime preparation and acceptance of new command + * resource combinations. + * @note `accept_command_resource_claim` is called on all actuators and system + * components. Hardware interfaces should return `hardware_interface::return_type::OK` + * by default. + * + * @param[in] start_interfaces A vector of string identifiers for the command interfaces starting. + * @param[in] stop_interfaces A vector of string identifiers for the command interfaces stopping. + * @return `true` if the switch can be prepared; `false` if a component rejects + * the switch request or if at least one of the input interfaces does not exist + * or is not available (i.e., the component is not in `ACTIVE` or `INACTIVE` state). + */ + + bool prepare_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces); - /// Notify the hardware components that realtime hardware mode switching should occur. - /** - * Hardware components are asked to perform the command interface mode switching. - * - * \note this is intended for mode-switching when a hardware interface needs to change - * control mode depending on which command interface is claimed. - * \note this is for realtime switching of the command interface. - * \note it is assumed that `prepare_command_mode_switch` is called just before this method - * with the same input arguments. - * \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 true if switch is performed, false if a component rejects switching. - */ + /// Notifies hardware components to perform realtime command interface mode switching. +/** + * @brief Triggers realtime switching of command interface modes in hardware components. + * + * @note Intended for mode-switching when a hardware interface changes control mode + * based on the claimed command interface. + * @note Assumes `prepare_command_mode_switch` was called just before this method + * with the same arguments. + * + * @param[in] start_interfaces Vector of command interface identifiers to start. + * @param[in] stop_interfaces Vector of command interface identifiers to stop. + * @return `true` if the switch is performed; `false` if rejected by a component. + */ + + bool perform_command_mode_switch( const std::vector & start_interfaces, const std::vector & stop_interfaces); - /// Sets state of hardware component. - /** - * Set set of hardware component if possible. - * Takes care of all transitions needed to reach the target state. - * It implements the state machine from: https://design.ros2.org/articles/node_lifecycle.html - * - * The method is not part of the real-time critical update loop. - * - * \param[in] component_name component name to change state. - * \param[in] target_state target state to set for a hardware component. - * \return hardware_interface::retun_type::OK if component successfully switched its state and - * hardware_interface::return_type::ERROR any of state transitions has failed. - */ + /// Sets the state of a hardware component. +/** + * @brief Transitions a hardware component to the specified state. + * + * Handles all necessary state transitions to reach the target state, following + * the state machine defined in: https://design.ros2.org/articles/node_lifecycle.html. + * + * @note This method is not part of the real-time critical update loop. + * + * @param[in] component_name Name of the hardware component to update. + * @param[in] target_state Desired state for the hardware component. + * @return `hardware_interface::return_type::OK` if the state transition succeeds, + * `hardware_interface::return_type::ERROR` if any transition fails. + */ + + return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); - /// Reads all loaded hardware components. - /** - * Reads from all active hardware components. - * - * Part of the real-time critical update loop. - * It is realtime-safe if used hardware interfaces are implemented adequately. - */ + /// Reads data from all loaded hardware components. +/** + * @brief Collects data from all active hardware components. + * + * This function is part of the real-time critical update loop and is + * real-time safe if the hardware interfaces are correctly implemented. + */ + + HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Write all loaded hardware components. - /** - * Writes to all active hardware components. - * - * Part of the real-time critical update loop. - * It is realtime-safe if used hardware interfaces are implemented adequately. - */ +/// Writes data to all loaded hardware components. +/** + * @brief Sends data to all active hardware components. + * + * This function is part of the real-time critical update loop and is + * real-time safe if the hardware interfaces are correctly implemented. + */ + + HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period); - /// Checks whether a command interface is registered under the given key. - /** - * \param[in] key string identifying the interface to check. - * \return true if interface exist, false otherwise. - */ + /// Checks if a command interface is registered under the given key. +/** + * @brief Verifies the existence of a command interface. + * + * @param[in] key String identifier of the command interface to check. + * @return `true` if the interface exists, `false` otherwise. + */ + bool command_interface_exists(const std::string & key) const; - /// Checks whether a state interface is registered under the given key. - /** - * \return true if interface exist, false otherwise. - */ + /// Checks if a state interface is registered under the given key. +/** + * @brief Verifies the existence of a state interface. + * + * @return `true` if the interface exists, `false` otherwise. + */ + bool state_interface_exists(const std::string & key) const; protected: - /// Gets the logger for the resource manager - /** - * \return logger of the resource manager - */ + /// Retrieves the logger for the resource manager. +/** + * @brief Gets the logger instance used by the resource manager. + * + * @return The logger of the resource manager. + */ + rclcpp::Logger get_logger() const; - /// Gets the clock for the resource manager - /** - * \return clock of the resource manager - */ + /// Retrieves the clock for the resource manager. +/** + * @brief Gets the clock instance used by the resource manager. + * + * @return The clock of the resource manager. + */ + rclcpp::Clock::SharedPtr get_clock() const; bool components_are_loaded_and_initialized_ = false; diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 44a22049ee..d40f3b5eb3 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -42,36 +42,31 @@ namespace hardware_interface using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; -/// Virtual Class to implement when integrating a stand-alone sensor into ros2_control. +/// Virtual class for integrating a stand-alone sensor into ros2_control. /** - * The typical examples are Force-Torque Sensor (FTS), Interial Measurement Unit (IMU). + * @brief Base class for standalone sensor integration in ros2_control. * - * Methods return values have type - * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following - * meaning: + * Typical examples include Force-Torque Sensors (FTS) and Inertial Measurement Units (IMU). * - * \returns CallbackReturn::SUCCESS method execution was successful. - * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. - * \returns CallbackReturn::ERROR critical error has happened that should be managed in - * "on_error" method. + * Methods return values of type + * `rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn` with the following meanings: * - * The hardware ends after each method in a state with the following meaning: + * @returns `CallbackReturn::SUCCESS` - Method executed successfully. + * @returns `CallbackReturn::FAILURE` - Execution failed but can be retried. + * @returns `CallbackReturn::ERROR` - Critical error requiring handling in `on_error`. * - * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is - * available. + * Hardware states after each method execution: * - * INACTIVE (on_configure, on_deactivate): - * Communication with the hardware is started and it is configured. - * States can be read. - * - * FINALIZED (on_shutdown): - * Hardware interface is ready for unloading/destruction. - * Allocated memory is cleaned up. - * - * ACTIVE (on_activate): + * - **UNCONFIGURED** (on_init, on_cleanup): + * Hardware is initialized but not communicating; no interfaces are available. + * - **INACTIVE** (on_configure, on_deactivate): + * Communication is established, hardware is configured, and states can be read. + * - **FINALIZED** (on_shutdown): + * Hardware interface is ready for unloading; allocated memory is cleaned up. + * - **ACTIVE** (on_activate): * States can be read. */ + class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: @@ -82,26 +77,31 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { } - /// 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. - */ + /// Deleted copy constructor for SensorInterface. +/** + * @brief Prevents copying of SensorInterface to ensure unique ownership. + * + * Hardware interfaces require unique ownership to prevent failed or simultaneous + * access to hardware resources. + */ + SensorInterface(const SensorInterface & other) = delete; SensorInterface(SensorInterface && other) = delete; virtual ~SensorInterface() = 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_interface pointer to the clock interface. - * \param[in] logger_interface pointer to the logger interface. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ + /// Initializes the hardware interface using data from the robot's URDF, clock, and logger. +/** + * @brief Sets up the hardware interface with parsed URDF data and system interfaces. + * + * @param[in] hardware_info Structure containing data from the URDF. + * @param[in] clock_interface Pointer to the clock interface. + * @param[in] logger_interface Pointer to the logger interface. + * @return `CallbackReturn::SUCCESS` if initialization succeeds. + * @return `CallbackReturn::ERROR` if data is missing or an error occurs. + */ + CallbackReturn init( const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) @@ -122,12 +122,15 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return on_init(hardware_info); }; - /// 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. - */ + /// Initializes the hardware interface from parsed robot URDF data. +/** + * @brief Sets up the hardware interface using the provided URDF data. + * + * @param[in] hardware_info Structure containing data from the URDF. + * @return `CallbackReturn::SUCCESS` if data is successfully parsed and available. + * @return `CallbackReturn::ERROR` if data is missing or an error occurs. + */ + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; @@ -135,19 +138,20 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI parse_state_interface_descriptions(info_.sensors, sensor_state_interfaces_); return CallbackReturn::SUCCESS; }; +/// Exports all state interfaces for this hardware interface. +/** + * @brief Exports state interfaces, either via return value or through `on_export_state_interfaces()`. + * + * If an empty vector is returned, the `on_export_state_interfaces()` method is invoked. + * If a vector with state interfaces is returned, the state interfaces are exported directly, + * and ownership is transferred to the resource manager. Afterward, functions like `set_command(...)`, + * `get_command(...)`, etc., cannot be used. + * + * @note Ownership of the state interfaces is transferred to the caller. + * + * @return A vector of state interfaces. + */ - /// 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 " @@ -160,12 +164,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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 - */ +/// Override to export custom StateInterfaces not defined in the URDF. +/** + * @brief Adds custom StateInterfaces to the unlisted_state_interfaces_ map. + * + * Override this method to export additional StateInterfaces that are not defined in the URDF file. + * These interfaces will be added to the `unlisted_state_interfaces_` map. + * + * @return A vector of descriptions for the unlisted StateInterfaces. + */ + virtual std::vector export_unlisted_state_interface_descriptions() { @@ -173,13 +181,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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 - */ +/// Default implementation for exporting StateInterfaces. +/** + * @brief Creates and stores StateInterfaces based on the InterfaceDescription. + * + * This default implementation generates the StateInterfaces and assigns the memory used by the controllers + * and hardware to reside in the `sensor_interface`. + * + * @return A vector of shared pointers to the created and stored StateInterfaces. + */ + virtual std::vector on_export_state_interfaces() { // import the unlisted interfaces @@ -222,16 +233,18 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return state_interfaces; } - /// 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. - */ +/// Triggers the read method synchronously or asynchronously based on HardwareInfo. +/** + * @brief Updates the state interfaces with the latest data from the physical hardware. + * + * The data readings from the hardware must be updated and reflected in the exported state interfaces. + * Specifically, the data pointed to by the interfaces will be updated. + * + * @param[in] time The time at the start of the control loop iteration. + * @param[in] period The duration of 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) { @@ -269,34 +282,45 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - */ + /// Reads the current state values from the actuator. +/** + * @brief Updates the state interfaces with the latest data from the actuator. + * + * The data readings from the physical hardware must be updated and reflected in the exported state interfaces. + * Specifically, the data pointed to by the interfaces will be updated. + * + * @param[in] time The time at the start of the control loop iteration. + * @param[in] period The duration of 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. - */ + /// Gets the name of the actuator hardware. +/** + * @brief Retrieves the name of the actuator hardware. + * + * @return The name of the actuator hardware. + */ + const std::string & get_name() const { return info_.name; } - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ + /// Gets the name of the actuator hardware group. +/** + * @brief Retrieves the name of the hardware group to which the actuator belongs. + * + * @return The name of the actuator hardware group. + */ + const std::string & get_group_name() const { return info_.group; } - /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ + /// Gets the lifecycle state of the actuator hardware. +/** + * @brief Retrieves the lifecycle state of the actuator hardware. + * + * @return The lifecycle state of the actuator hardware. + */ + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. @@ -318,28 +342,40 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return sensor_states_map_.at(interface_name)->get_value(); } - /// Get the logger of the SensorInterface. - /** - * \return logger of the SensorInterface. - */ + /// Gets the logger of the SensorInterface. +/** + * @brief Retrieves the logger instance associated with the SensorInterface. + * + * @return The logger of the SensorInterface. + */ + rclcpp::Logger get_logger() const { return sensor_logger_; } - /// Get the clock of the SensorInterface. - /** - * \return clock of the SensorInterface. - */ + /// Gets the clock of the SensorInterface. +/** + * @brief Retrieves the clock instance associated with the SensorInterface. + * + * @return The clock of the SensorInterface. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } - /// Get the hardware info of the SensorInterface. - /** - * \return hardware info of the SensorInterface. - */ +/// Gets the hardware info of the SensorInterface. +/** + * @brief Retrieves the hardware information associated with the SensorInterface. + * + * @return The 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. - */ + /// Enables or disables introspection of the sensor hardware. +/** + * @brief Controls the introspection of the sensor hardware. + * + * @param[in] enable `true` to enable introspection, `false` to disable it. + */ + void enable_introspection(bool enable) { if (enable) diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index d4d2505849..f4ae0c8511 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -46,52 +46,42 @@ namespace hardware_interface using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /** - * @brief Virtual Class to implement when integrating a complex system into ros2_control. + * @brief Virtual class to implement when integrating a complex system into ros2_control. * - * The common examples for these types of hardware are multi-joint systems with or without sensors - * such as industrial or humanoid robots. + * This class is typically used for multi-joint systems with or without sensors, such as industrial or humanoid robots. * - * Methods return values have type - * rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn with the following - * meaning: + * Methods return values of type + * `rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn` with the following meanings: * - * \returns CallbackReturn::SUCCESS method execution was successful. - * \returns CallbackReturn::FAILURE method execution has failed and and can be called again. - * \returns CallbackReturn::ERROR critical error has happened that should be managed in - * "on_error" method. + * @returns `CallbackReturn::SUCCESS` - Method execution was successful. + * @returns `CallbackReturn::FAILURE` - Method execution failed, but can be called again. + * @returns `CallbackReturn::ERROR` - A critical error occurred and should be handled in the `on_error` method. * - * The hardware ends after each method in a state with the following meaning: + * Hardware states after each method execution: * - * UNCONFIGURED (on_init, on_cleanup): - * Hardware is initialized but communication is not started and therefore no interface is - *available. + * - **UNCONFIGURED** (on_init, on_cleanup): + * Hardware is initialized but communication is not started, so no interfaces are available. * - * INACTIVE (on_configure, on_deactivate): - * Communication with the hardware is started and it is configured. - * States can be read and command interfaces are available. + * - **INACTIVE** (on_configure, on_deactivate): + * Communication with the hardware is started and configured. States can be read, and command interfaces are available. + * Hardware components can choose to continue using commands from the `CommandInterfaces` or skip them. * - * As of now, it is left to the hardware component implementation to continue using the command - *received from the ``CommandInterfaces`` or to skip them completely. + * - **FINALIZED** (on_shutdown): + * Hardware interface is ready for unloading/destruction, and memory is cleaned up. * - * FINALIZED (on_shutdown): - * Hardware interface is ready for unloading/destruction. - * Allocated memory is cleaned up. + * - **ACTIVE** (on_activate): + * Power circuits of hardware are active, and hardware can be moved (e.g., brakes are disabled). Command interfaces are available. * - * ACTIVE (on_activate): - * Power circuits of hardware are active and hardware can be moved, e.g., brakes are disabled. - * Command interfaces available. - * - * \todo - * Implement + * @todo + * Implement the following: * * https://github.com/ros-controls/ros2_control/issues/931 * * https://github.com/ros-controls/roadmap/pull/51/files - * * this means in INACTIVE state: - * * States can be read and non-movement hardware interfaces commanded. - * * Hardware interfaces for movement will NOT be available. - * * Those interfaces are: HW_IF_POSITION, HW_IF_VELOCITY, HW_IF_ACCELERATION, and - *HW_IF_EFFORT. + * * In the **INACTIVE** state: + * * States can be read, and non-movement hardware interfaces can be commanded. + * * Hardware interfaces for movement (e.g., `HW_IF_POSITION`, `HW_IF_VELOCITY`, `HW_IF_ACCELERATION`, and `HW_IF_EFFORT`) will NOT be available. */ + class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface { public: @@ -102,26 +92,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI { } - /// 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 copy constructor is deleted. +/** + * @brief Prevents copying of the SystemInterface to ensure unique ownership of hardware interfaces. + * + * Hardware interfaces are designed with unique ownership to avoid issues with 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_interface pointer to the clock interface. - * \param[in] logger_interface pointer to the logger interface. - * \returns CallbackReturn::SUCCESS if required data are provided and can be parsed. - * \returns CallbackReturn::ERROR if any error happens or data are missing. - */ +/// Initializes the hardware interface from data parsed from the robot's URDF, clock, and logger interfaces. +/** + * @brief Sets up the hardware interface using data from the URDF, clock, and logger interfaces. + * + * @param[in] hardware_info Structure containing data from the URDF. + * @param[in] clock_interface Pointer to the clock interface. + * @param[in] logger_interface Pointer to the logger interface. + * @return `CallbackReturn::SUCCESS` if the required data are provided and can be parsed successfully. + * @return `CallbackReturn::ERROR` if any error occurs or if required data are missing. + */ + CallbackReturn init( const HardwareInfo & hardware_info, rclcpp::Logger logger, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) @@ -163,12 +157,15 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI return on_init(hardware_info); }; - /// 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. - */ + /// Initializes the hardware interface from data parsed from the robot's URDF. +/** + * @brief Sets up the hardware interface using data from the URDF. + * + * @param[in] hardware_info Structure containing data from the URDF. + * @return `CallbackReturn::SUCCESS` if the required data are provided and can be parsed successfully. + * @return `CallbackReturn::ERROR` if any error occurs or if required data are missing. + */ + virtual CallbackReturn on_init(const HardwareInfo & hardware_info) { info_ = hardware_info; @@ -181,17 +178,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI }; /// 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 - */ +/** + * @brief Exports state interfaces for the hardware interface, either using the old method or a custom vector. + * + * If an empty vector is returned, the `on_export_state_interfaces()` method is called. If a vector with `StateInterfaces` is returned, + * the export process is handled by this function, and ownership of the state interfaces is transferred to the resource manager. + * Once ownership is transferred, methods like `set_command(...)`, `get_command(...)`, and others can no longer be used. + * + * @note Ownership of the state interfaces is transferred to the caller. + * + * @return A vector of state interfaces. + */ + [[deprecated( "Replaced by vector on_export_state_interfaces() method. " "Exporting is handled " @@ -272,17 +270,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } /// 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 - */ +/** + * @brief Exports command interfaces for the hardware interface, either using the old method or a custom vector. + * + * If an empty vector is returned, the `on_export_command_interfaces()` method is called. If a vector with `CommandInterfaces` is returned, + * the export process is handled by this function, and ownership of the command interfaces is transferred to the resource manager. + * Once ownership is transferred, methods like `set_command(...)`, `get_command(...)`, and others can no longer be used. + * + * @note Ownership of the command interfaces is transferred to the caller. + * + * @return A vector of command interfaces. + */ + [[deprecated( "Replaced by vector on_export_command_interfaces() method. " "Exporting is " @@ -357,19 +356,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \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. - */ + /// Prepare for a new command interface switch. +/** + * @brief Prepares the system for any mode-switching required by the new command interface combination. + * + * This is a non-realtime evaluation to determine if the new set of command interface claims are possible. + * It also starts preparing the data structures for the upcoming switch. + * + * @note This is a non-realtime process. All starting and stopping interface keys are passed to all components, + * so the function should return `return_type::OK` by default if the interface keys are not relevant for the component. + * + * @param[in] start_interfaces A vector of string identifiers for the command interfaces starting. + * @param[in] stop_interfaces A 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. + * @return `return_type::ERROR` if preparation cannot proceed. + */ + virtual return_type prepare_command_mode_switch( const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) @@ -377,18 +379,22 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - * \note All starting and stopping interface keys are passed to all components, so the function - * should return return_type::OK by default when given interface keys not relevant for this - * component. \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. - */ + /// Perform switching to the new command interface. +/** + * @brief Performs mode-switching for the new command interface combination. + * + * This operation is part of the real-time update loop and should be executed quickly. + * + * @note All starting and stopping interface keys are passed to all components, so the function should return + * `return_type::OK` by default if the interface keys are not relevant for the component. + * + * @param[in] start_interfaces A vector of string identifiers for the command interfaces starting. + * @param[in] stop_interfaces A 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. + * @return `return_type::ERROR` if the switching cannot proceed. + */ + virtual return_type perform_command_mode_switch( const std::vector & /*start_interfaces*/, const std::vector & /*stop_interfaces*/) @@ -396,17 +402,18 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - */ + /// Triggers the read method synchronously or asynchronously depending on the HardwareInfo. +/** + * @brief Updates data readings from the physical hardware and reflects them in the exported state interfaces. + * + * The data pointed to by the interfaces is updated with the latest readings. This method is called within + * 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) { @@ -444,28 +451,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - */ + /// Read the current state values from the actuator. +/** + * @brief Updates the data readings from the physical actuator and reflects them in the exported state interfaces. + * + * The data pointed to by the interfaces is updated with the current state values. + * + * @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. - */ + /// Triggers the write method synchronously or asynchronously depending on the HardwareInfo. +/** + * @brief Updates the physical hardware with the latest values from the exported command interfaces. + * + * This method is called in the resource manager's write loop to reflect the most recent commands. + * + * @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 write was successful, `return_type::ERROR` otherwise. + */ + HardwareComponentCycleStatus trigger_write( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -492,38 +501,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI 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. - */ + /// Write the current command values to the actuator. +/** + * @brief Updates the physical actuator with the latest command values from the exported command interfaces. + * + * The hardware is updated with the most recent command values to ensure the actuator performs as expected. + * + * @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 write 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. - */ + /// Get name of the actuator hardware. +/** + * @brief Retrieves the name of the actuator hardware. + * + * @return The name of the actuator hardware. + */ + const std::string & get_name() const { return info_.name; } - /// Get name of the actuator hardware group to which it belongs to. - /** - * \return group name. - */ + /// Get name of the actuator hardware group to which it belongs. +/** + * @brief Retrieves the name of the actuator hardware group. + * + * @return The name of the actuator hardware group. + */ + const std::string & get_group_name() const { return info_.group; } /// Get life-cycle state of the actuator hardware. - /** - * \return state. - */ +/** + * @brief Retrieves the life-cycle state of the actuator hardware. + * + * @return The current life-cycle state of the actuator hardware. + */ + const rclcpp_lifecycle::State & get_lifecycle_state() const { return lifecycle_state_; } /// Set life-cycle state of the actuator hardware. /** - * \return state. + * @return state. */ void set_lifecycle_state(const rclcpp_lifecycle::State & new_state) { @@ -551,27 +571,34 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI } /// Get the logger of the SystemInterface. - /** - * \return logger of the SystemInterface. - */ +/** + * @brief Retrieves the logger associated with the SystemInterface. + * + * @return The logger of the SystemInterface. + */ + rclcpp::Logger get_logger() const { return system_logger_; } /// Get the clock of the SystemInterface. /** - * \return clock of the SystemInterface. + * @return clock of the SystemInterface. */ rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } /// Get the hardware info of the SystemInterface. /** - * \return 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. - */ +/** + * @brief Prepares the hardware for activation by the resource manager. + * + * This method is called before the hardware is activated, allowing for any necessary setup or + * configuration before the hardware begins active operation. + */ + void prepare_for_activation() { read_return_info_.store(return_type::OK, std::memory_order_release); diff --git a/hardware_interface/include/mock_components/generic_system.hpp b/hardware_interface/include/mock_components/generic_system.hpp index 418924c007..8c50c29c38 100644 --- a/hardware_interface/include/mock_components/generic_system.hpp +++ b/hardware_interface/include/mock_components/generic_system.hpp @@ -61,13 +61,15 @@ class GenericSystem : public hardware_interface::SystemInterface } protected: - /// Use standard interfaces for joints because they are relevant for dynamic behavior - /** - * By splitting the standard interfaces from other type, the users are able to inherit this - * class and simply create small "simulation" with desired dynamic behavior. - * The advantage over using Gazebo is that enables "quick & dirty" tests of robot's URDF and - * controllers. - */ + /// Use standard interfaces for joints because they are relevant for dynamic behavior. +/** + * @brief Uses standard interfaces for joints to facilitate dynamic behavior. + * + * By separating standard interfaces from others, this approach allows users to inherit this + * class and quickly create simulations with desired dynamic behavior. The advantage over using + * Gazebo is that it enables "quick & dirty" tests of the robot's URDF and controllers. + */ + const std::vector standard_interfaces_ = { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_ACCELERATION, hardware_interface::HW_IF_EFFORT}; diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index e1ff42ee14..ca7ff6678a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -73,13 +73,15 @@ namespace hardware_interface { namespace detail { -/// Gets value of the text between tags. /** - * \param[in] element_it XMLElement iterator to search for the text. - * \param[in] tag_name parent tag name where text is searched for (used for error output) - * \return text of for the tag - * \throws std::runtime_error if text is not found + * @brief Retrieves the text value between XML tags. + * + * @param[in] element_it Iterator pointing to the XML element. + * @param[in] tag_name Parent tag name for error reporting. + * @return The text content of the tag. + * @throws std::runtime_error If text is not found. */ + std::string get_text_for_element( const tinyxml2::XMLElement * element_it, const std::string & tag_name) { @@ -92,16 +94,19 @@ std::string get_text_for_element( return get_text_output; } -/// Gets value of the attribute on an XMLelement. /** - * If attribute is not found throws an error. + * @brief Retrieves the value of an attribute from an XML element. + * + * This method searches for the specified attribute in the given XML element. + * If the attribute is not found, an error is thrown. * - * \param[in] element_it XMLElement iterator to search for the attribute - * \param[in] attribute_name attribute name to search for and return value - * \param[in] tag_name parent tag name where attribute is searched for (used for error output) - * \return attribute value - * \throws std::runtime_error if attribute is not found + * @param[in] element_it Iterator to the XML element being searched. + * @param[in] attribute_name Name of the attribute to retrieve. + * @param[in] tag_name Name of the parent tag where the attribute is searched (used for error output). + * @return The value of the specified attribute. + * @throws std::runtime_error If the attribute is not found. */ + std::string get_attribute_value( const tinyxml2::XMLElement * element_it, const char * attribute_name, std::string tag_name) { @@ -115,31 +120,37 @@ std::string get_attribute_value( return element_it->Attribute(attribute_name); } -/// Gets value of the attribute on an XMLelement. /** - * If attribute is not found throws an error. + * @brief Gets the value of an attribute from an XML element. + * + * This method searches for the specified attribute in the given XML element. + * If the attribute is not found, it throws an error. * - * \param[in] element_it XMLElement iterator to search for the attribute - * \param[in] attribute_name attribute name to search for and return value - * \param[in] tag_name parent tag name where attribute is searched for (used for error output) - * \return attribute value - * \throws std::runtime_error if attribute is not found + * @param[in] element_it Iterator pointing to the XML element being searched. + * @param[in] attribute_name Name of the attribute to retrieve. + * @param[in] tag_name Name of the parent tag where the attribute is searched (used for error messages). + * @return The value of the specified attribute. + * @throws std::runtime_error If the attribute is not found. */ + std::string get_attribute_value( const tinyxml2::XMLElement * element_it, const char * attribute_name, const char * tag_name) { return get_attribute_value(element_it, attribute_name, std::string(tag_name)); } -/// Gets value of the parameter on an XMLelement. /** - * If parameter is not found, returns specified default value + * @brief Retrieves the value of a parameter from an XML element. + * + * This method searches for the specified parameter in the given XML element. + * If the parameter is not found, the specified default value is returned. * - * \param[in] element_it XMLElement iterator to search for the attribute - * \param[in] attribute_name attribute name to search for and return value - * \param[in] default_value When the attribute is not found, this value is returned instead - * \return attribute value or default + * @param[in] element_it Iterator pointing to the XML element being searched. + * @param[in] attribute_name Name of the attribute to retrieve. + * @param[in] default_value Value to return if the attribute is not found. + * @return The value of the specified attribute or the default value if not found. */ + double get_parameter_value_or( const tinyxml2::XMLElement * params_it, const char * parameter_name, const double default_value) { @@ -169,15 +180,18 @@ double get_parameter_value_or( return default_value; } -/// Parse optional size attribute /** - * Parses an XMLElement and returns the value of the size attribute. - * If not specified, defaults to 1. If not given a positive integer, throws an error. + * @brief Parses an optional size attribute from an XML element. * - * \param[in] elem XMLElement that has the size attribute. - * \return The size. - * \throws std::runtime_error if not given a positive non-zero integer as value. + * This method retrieves the value of the size attribute from the given XML element. + * If the attribute is not specified, it defaults to 1. If the value is not a positive + * non-zero integer, an error is thrown. + * + * @param[in] elem XML element containing the size attribute. + * @return The parsed size value. + * @throws std::runtime_error If the value is not a positive non-zero integer. */ + std::size_t parse_size_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kSizeAttribute); @@ -205,14 +219,16 @@ std::size_t parse_size_attribute(const tinyxml2::XMLElement * elem) return size; } -/// Parse data_type attribute -/** - * Parses an XMLElement and returns the value of the data_type attribute. - * Defaults to "double" if not specified. - * - * \param[in] elem XMLElement that has the data_type attribute. - * \return string specifying the data type. - */ +//** +* @brief Parses the data_type attribute from an XML element. +* +* This method retrieves the value of the data_type attribute from the given XML element. +* If the attribute is not specified, it defaults to "double". +* +* @param[in] elem XML element containing the data_type attribute. +* @return A string specifying the data type. +*/ + std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kDataTypeAttribute); @@ -229,14 +245,16 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } -/// Parse rw_rate attribute /** - * Parses an XMLElement and returns the value of the rw_rate attribute. - * Defaults to 0 if not specified. + * @brief Parses the rw_rate attribute from an XML element. + * + * This method retrieves the value of the rw_rate attribute from the given XML element. + * If the attribute is not specified, it defaults to 0. * - * \param[in] elem XMLElement that has the rw_rate attribute. - * \return unsigned int specifying the read/write rate. + * @param[in] elem XML element containing the rw_rate attribute. + * @return An unsigned integer specifying the read/write rate. */ + unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kReadWriteRateAttribute); @@ -265,28 +283,32 @@ unsigned int parse_rw_rate_attribute(const tinyxml2::XMLElement * elem) } } -/// Parse is_async attribute /** - * Parses an XMLElement and returns the value of the is_async attribute. - * Defaults to "false" if not specified. + * @brief Parses the is_async attribute from an XML element. + * + * This method retrieves the value of the is_async attribute from the given XML element. + * If the attribute is not specified, it defaults to "false". * - * \param[in] elem XMLElement that has the data_type attribute. - * \return boolean specifying the if the value read was true or false. + * @param[in] elem XML element containing the is_async attribute. + * @return A boolean indicating whether the value read was true or false. */ + bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kIsAsyncAttribute); return attr ? parse_bool(attr->Value()) : false; } -/// Parse thread_priority attribute /** - * Parses an XMLElement and returns the value of the thread_priority attribute. - * Defaults to 50 if not specified. + * @brief Parses the thread_priority attribute from an XML element. * - * \param[in] elem XMLElement that has the thread_priority attribute. - * \return positive integer specifying the thread priority. + * This method retrieves the value of the thread_priority attribute from the given XML element. + * If the attribute is not specified, it defaults to 50. + * + * @param[in] elem XML element containing the thread_priority attribute. + * @return A positive integer specifying the thread priority. */ + int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) { const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); @@ -308,12 +330,17 @@ int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) } } -/// Search XML snippet from URDF for parameters. /** - * \param[in] params_it pointer to the iterator where parameters info should be found - * \return key-value map with parameters - * \throws std::runtime_error if a component attribute or tag is not found + * @brief Searches an XML snippet from URDF for parameters. + * + * This method scans the provided XML snippet for parameter information and + * returns a key-value map of the extracted parameters. + * + * @param[in] params_it Pointer to the iterator where parameter information should be found. + * @return A key-value map containing the extracted parameters. + * @throws std::runtime_error If a required component attribute or tag is not found. */ + std::unordered_map parse_parameters_from_xml( const tinyxml2::XMLElement * params_it) { @@ -337,13 +364,18 @@ std::unordered_map parse_parameters_from_xml( return parameters; } -/// Search XML snippet for definition of interfaceTypes. /** - * \param[in] interfaces_it pointer to the iterator over interfaces - * \param[in] interfaceTag interface type tag (command or state) - * \return list of interface types - * \throws std::runtime_error if the interfaceType text not set in a tag + * @brief Searches an XML snippet for the definition of interface types. + * + * This method scans the provided XML snippet to extract interface types based + * on the specified interface tag (command or state). + * + * @param[in] interfaces_it Pointer to the iterator over interfaces. + * @param[in] interfaceTag The interface type tag (command or state). + * @return A list of interface types. + * @throws std::runtime_error If the interfaceType text is not set in a tag. */ + hardware_interface::InterfaceInfo parse_interfaces_from_xml( const tinyxml2::XMLElement * interfaces_it) { @@ -397,13 +429,17 @@ hardware_interface::InterfaceInfo parse_interfaces_from_xml( return interface; } -/// Search XML snippet from URDF for information about a control component. /** - * \param[in] component_it pointer to the iterator where component - * info should be found - * \return ComponentInfo filled with information about component - * \throws std::runtime_error if a component attribute or tag is not found + * @brief Searches an XML snippet from URDF for information about a control component. + * + * This method scans the provided XML snippet to extract information about + * a control component and returns a ComponentInfo structure containing the details. + * + * @param[in] component_it Pointer to the iterator where component information should be found. + * @return A ComponentInfo object filled with details about the component. + * @throws std::runtime_error If a required component attribute or tag is not found. */ + ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it) { ComponentInfo component; @@ -465,15 +501,16 @@ ComponentInfo parse_component_from_xml(const tinyxml2::XMLElement * component_it return component; } -/// Search XML snippet from URDF for information about a complex component. /** + * @brief Searches an XML snippet from URDF for information about a complex component. + * * A complex component can have a non-double data type specified on its interfaces, - * and the interface may be an array of a fixed size of the data type. + * and the interface may be an array with a fixed size of the given data type. * - * \param[in] component_it pointer to the iterator where component - * info should be found - * \throws std::runtime_error if a required component attribute or tag is not found. + * @param[in] component_it Pointer to the iterator where component information should be found. + * @throws std::runtime_error If a required component attribute or tag is not found. */ + ComponentInfo parse_complex_component_from_xml(const tinyxml2::XMLElement * component_it) { ComponentInfo component; @@ -539,12 +576,17 @@ ActuatorInfo parse_transmission_actuator_from_xml(const tinyxml2::XMLElement * e return actuator_info; } -/// Search XML snippet from URDF for information about a transmission. /** - * \param[in] transmission_it pointer to the iterator where transmission info should be found - * \return TransmissionInfo filled with information about transmission - * \throws std::runtime_error if an attribute or tag is not found + * @brief Searches an XML snippet from URDF for information about a transmission. + * + * This method scans the provided XML snippet to extract information about + * a transmission and returns a TransmissionInfo structure containing the details. + * + * @param[in] transmission_it Pointer to the iterator where transmission information should be found. + * @return A TransmissionInfo object filled with details about the transmission. + * @throws std::runtime_error If a required attribute or tag is not found. */ + TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transmission_it) { TransmissionInfo transmission; @@ -580,11 +622,16 @@ TransmissionInfo parse_transmission_from_xml(const tinyxml2::XMLElement * transm return transmission; } -/// Auto-fill some contents of transmission info based on context /** - * \param[in,out] hardware HardwareInfo structure with elements already parsed. - * \throws std::runtime_error + * @brief Auto-fills certain contents of the transmission info based on context. + * + * This method populates relevant fields in the given HardwareInfo structure + * using the available parsed elements. + * + * @param[in,out] hardware A HardwareInfo structure with elements already parsed. + * @throws std::runtime_error If an error occurs during the auto-fill process. */ + void auto_fill_transmission_interfaces(HardwareInfo & hardware) { for (auto & transmission : hardware.transmissions) @@ -634,13 +681,17 @@ void auto_fill_transmission_interfaces(HardwareInfo & hardware) } } -/// Parse a control resource from an "ros2_control" tag. /** - * \param[in] ros2_control_it pointer to ros2_control element - * with information about resource. - * \return HardwareInfo filled with information about the robot - * \throws std::runtime_error if a attributes or tag are not found + * @brief Parses a control resource from a "ros2_control" tag. + * + * This method extracts hardware-related information from a given "ros2_control" + * XML element and returns a HardwareInfo structure containing details about the robot. + * + * @param[in] ros2_control_it Pointer to the ros2_control element containing resource information. + * @return A HardwareInfo object filled with details about the robot. + * @throws std::runtime_error If required attributes or tags are not found. */ + HardwareInfo parse_resource_from_xml( const tinyxml2::XMLElement * ros2_control_it, const std::string & urdf) { diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 80f50fa73d..d91717d8b3 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -672,17 +672,19 @@ class ResourceStorage interface->registerIntrospection(); return interface_name; } - /// Adds exported state interfaces into internal storage. /** - * Adds state interfaces to the internal storage. State interfaces exported from hardware or - * chainable controllers are moved to the map with name-interface pairs and available list's - * size is increased to reserve storage when interface change theirs status in real-time - * control loop. - * - * \param[interfaces] list of state interface to add into storage. - * \returns list of interface names that are added into internal storage. The output is used to - * avoid additional iterations to cache interface names, e.g., for initializing info structures. - */ + * @brief Adds exported state interfaces into internal storage. + * + * This method moves state interfaces exported from hardware or chainable controllers + * into an internal storage map with name-interface pairs. Additionally, it increases + * the available list's size to reserve storage for real-time control loop updates. + * + * @param[in] interfaces List of state interfaces to add into storage. + * @return A list of interface names that have been added to internal storage. + * This output helps avoid extra iterations for caching interface names, + * such as when initializing info structures. + */ + std::vector add_state_interfaces( std::vector & interfaces) { @@ -707,12 +709,15 @@ class ResourceStorage return interface_names; } - /// Removes state interfaces from internal storage. /** - * State interface are removed from the maps with theirs storage and their claimed status. - * - * \param[interface_names] list of state interface names to remove from storage. - */ + * @brief Removes state interfaces from internal storage. + * + * This method removes state interfaces from internal storage maps, including their + * allocated storage and claimed status. + * + * @param[in] interface_names List of state interface names to remove from storage. + */ + void remove_state_interfaces(const std::vector & interface_names) { for (const auto & interface : interface_names) @@ -722,17 +727,20 @@ class ResourceStorage } } - /// Adds exported command interfaces into internal storage. /** - * Add command interfaces to the internal storage. Command interfaces exported from hardware or - * chainable controllers are moved to the map with name-interface pairs, the interface names are - * added to the claimed map and available list's size is increased to reserve storage when - * interface change theirs status in real-time control loop. - * - * \param[interfaces] list of command interface to add into storage. - * \returns list of interface names that are added into internal storage. The output is used to - * avoid additional iterations to cache interface names, e.g., for initializing info structures. - */ + * @brief Adds exported command interfaces into internal storage. + * + * This method moves command interfaces exported from hardware or chainable controllers + * into an internal storage map with name-interface pairs. Additionally, it adds the + * interface names to the claimed map and increases the available list's size to reserve + * storage for real-time control loop updates. + * + * @param[in] interfaces List of command interfaces to add into storage. + * @return A list of interface names that have been added to internal storage. + * This output helps avoid extra iterations for caching interface names, + * such as when initializing info structures. + */ + std::vector add_command_interfaces(std::vector & interfaces) { std::vector interface_names; @@ -768,12 +776,15 @@ class ResourceStorage return interface_names; } - /// Removes command interfaces from internal storage. - /** - * Command interface are removed from the maps with theirs storage and their claimed status. - * - * \param[interface_names] list of command interface names to remove from storage. - */ + /** + * @brief Removes command interfaces from internal storage. + * + * This method removes command interfaces from internal storage maps, including their + * allocated storage and claimed status. + * + * @param[in] interface_names List of command interface names to remove from storage. + */ + void remove_command_interfaces(const std::vector & interface_names) { for (const auto & interface : interface_names) @@ -962,16 +973,21 @@ class ResourceStorage return hw_group_state_.at(group_name); } - /// Gets the logger for the resource storage /** - * \return logger of the resource storage - */ + * @brief Gets the logger for the resource storage. + * + * This method retrieves the logger instance associated with the resource storage. + * + * @return The logger of the resource storage. + */ + const rclcpp::Logger & get_logger() const { return rm_logger_; } - /// Gets the clock for the resource storage /** - * \return clock of the resource storage - */ + * @brief Gets the clock for the resource storage. + * @return The clock of the resource storage. + */ + rclcpp::Clock::SharedPtr get_clock() const { return clock_interface_->get_clock(); } // hardware plugins diff --git a/joint_limits/include/joint_limits/joint_limiter_interface.hpp b/joint_limits/include/joint_limits/joint_limiter_interface.hpp index 27883b8937..81c4d35a2f 100644 --- a/joint_limits/include/joint_limits/joint_limiter_interface.hpp +++ b/joint_limits/include/joint_limits/joint_limiter_interface.hpp @@ -38,19 +38,22 @@ class JointLimiterInterface virtual ~JointLimiterInterface() = default; - /// Initialization of every JointLimiter. /** - * Initialization of JointLimiter for defined joints with their names. - * Robot description topic provides a topic name where URDF of the robot can be found. - * This is needed to use joint limits from URDF (not implemented yet!). - * Override this method only if initialization and reading joint limits should be adapted. - * Otherwise, initialize your custom limiter in `on_limit` method. - * - * \param[in] joint_names names of joints where limits should be applied. - * \param[in] param_itf node parameters interface object to access parameters. - * \param[in] logging_itf node logging interface to log if error happens. - * \param[in] robot_description_topic string of a topic where robot description is accessible. - */ + * @brief Initializes JointLimiter for defined joints. + * + * This method initializes JointLimiter for specified joints using their names. + * The robot description topic provides a source for retrieving the URDF of the robot, + * which is required for using joint limits (not implemented yet). + * + * Override this method only if joint limit initialization needs adaptation. + * Otherwise, initialize custom limiters in the `on_limit` method. + * + * @param[in] joint_names Names of joints where limits should be applied. + * @param[in] param_itf Node parameters interface for accessing parameters. + * @param[in] logging_itf Node logging interface for error logging. + * @param[in] robot_description_topic Topic name where the robot description is accessible. + */ + virtual bool init( const std::vector & joint_names, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -223,34 +226,42 @@ class JointLimiterInterface */ virtual bool on_configure(const JointLimitsStateDataType & current_joint_states) = 0; - /** \brief Method is realized by an implementation. - * - * Filter-specific implementation of the joint limits enforce algorithm for multiple dependent - * physical quantities. - * - * \param[in] current_joint_states current joint states a robot is in. - * \param[in,out] desired_joint_states joint state that should be adjusted to obey the limits. - * \param[in] dt time delta to calculate missing integrals and derivation in joint limits. - * \returns true if limits are enforced, otherwise false. - */ + /** + * @brief Method implemented by a specific realization. + * + * This method enforces joint limits for multiple dependent physical quantities based on the filter-specific implementation. + * + * @param[in] current_joint_states The current states of the joints of the robot. + * @param[in,out] desired_joint_states The joint state to be adjusted to ensure it respects the limits. + * @param[in] dt Time delta used to calculate missing integrals and derivatives in joint limits. + * + * @return true if the limits were successfully enforced, otherwise false. + */ + virtual bool on_enforce( const JointLimitsStateDataType & current_joint_states, JointLimitsStateDataType & desired_joint_states, const rclcpp::Duration & dt) = 0; - /** \brief Checks if the logging interface is set. - * \returns true if the logging interface is available, otherwise false. - * - * \note this way of interfacing would be useful for instances where the logging interface is not - * available, for example in the ResourceManager or ResourceStorage classes. - */ + /** + * @brief Checks if the logging interface is set. + * + * @return true if the logging interface is available, otherwise false. + * + * @note This approach is useful in cases where the logging interface is not available, + * such as in the ResourceManager or ResourceStorage classes. + */ + bool has_logging_interface() const { return node_logging_itf_ != nullptr; } - /** \brief Checks if the parameter interface is set. - * \returns true if the parameter interface is available, otherwise false. - * - * * \note this way of interfacing would be useful for instances where the logging interface is - * not available, for example in the ResourceManager or ResourceStorage classes. - */ + /** + * @brief Checks if the parameter interface is set. + * + * @return true if the parameter interface is available, otherwise false. + * + * @note This approach is useful in cases where the logging interface is not available, + * such as in the ResourceManager or ResourceStorage classes. + */ + bool has_parameter_interface() const { return node_param_itf_ != nullptr; } size_t number_of_joints_; diff --git a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp index 379cee0357..1d6985b6d8 100644 --- a/joint_limits/include/joint_limits/joint_limits_rosparam.hpp +++ b/joint_limits/include/joint_limits/joint_limits_rosparam.hpp @@ -27,13 +27,15 @@ namespace // utilities { -/// Declare and initialize a parameter with a type. /** - * - * Wrapper function for templated node's declare_parameter() which checks if - * parameter is already declared. - * For use in all components that inherit from ControllerInterface + * @brief Declare and initialize a parameter with a specific type. + * + * This is a wrapper function for the templated node's declare_parameter() method. It ensures that the parameter is declared only if it hasn't been declared previously. + * + * @note This function is intended for use in all components that inherit from ControllerInterface. */ + + template auto auto_declare( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -51,10 +53,10 @@ auto auto_declare( namespace joint_limits { /** - * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name using node - * parameters interface \p param_itf. - * - * The following parameter structure is declared with base name `joint_limits.joint_name`: + * @brief Declare JointLimits and SoftJointLimits parameters for a specific joint. + * + * This method declares a set of parameters for a joint with the name `joint_name` using the node's parameters interface `param_itf`. + * The following parameter structure is declared under the base name `joint_limits.joint_name`: * \code * has_position_limits: bool * min_position: double @@ -77,12 +79,13 @@ namespace joint_limits * soft_upper_limit: double * \endcode * - * \param[in] joint_name name of the joint for which parameters will be declared. - * \param[in] param_itf node parameters interface object to access parameters. - * \param[in] logging_itf node logging interface to log if error happens. - * - * \returns True if parameters are successfully declared, false otherwise. + * @param[in] joint_name Name of the joint for which parameters will be declared. + * @param[in] param_itf Node parameters interface to access parameters. + * @param[in] logging_itf Node logging interface to log errors if any occur. + * + * @returns `true` if parameters are successfully declared, `false` otherwise. */ + inline bool declare_parameters( const std::string & joint_name, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -131,16 +134,17 @@ inline bool declare_parameters( } /** - * Declare JointLimits and SoftJointLimits parameters for joint with \p joint_name for the \p node - * object. - * This is a convenience function. - * For parameters structure see the underlying `declare_parameters` function. - * - * \param[in] joint_name name of the joint for which parameters will be declared. - * \param[in] node node for parameters should be declared. + * @brief Declare JointLimits and SoftJointLimits parameters for a specific joint. + * + * This is a convenience function that declares a set of parameters for a joint with the name `joint_name` using the provided `node` object. + * For the detailed parameter structure, refer to the underlying `declare_parameters` function. * - * \returns True if parameters are successfully declared, false otherwise. + * @param[in] joint_name Name of the joint for which parameters will be declared. + * @param[in] node Node object for which parameters should be declared. + * + * @returns `true` if parameters are successfully declared, `false` otherwise. */ + inline bool declare_parameters(const std::string & joint_name, const rclcpp::Node::SharedPtr & node) { return declare_parameters( @@ -148,15 +152,17 @@ inline bool declare_parameters(const std::string & joint_name, const rclcpp::Nod } /** - * Declare JointLimits and SoftJointLimits parameters for joint with joint_name for the - * lifecycle_node object. This is a convenience function. For parameters structure see the - * underlying `declare_parameters` function. - * - * \param[in] joint_name name of the joint for which parameters will be declared. - * \param[in] lifecycle_node lifecycle node for parameters should be declared. + * @brief Declare JointLimits and SoftJointLimits parameters for a specific joint. + * + * This is a convenience function that declares a set of parameters for a joint with the name `joint_name` + * using the provided `lifecycle_node` object. For the detailed parameter structure, refer to the underlying `declare_parameters` function. * - * \returns True if parameters are successfully declared, false otherwise. + * @param[in] joint_name Name of the joint for which parameters will be declared. + * @param[in] lifecycle_node Lifecycle node for which parameters should be declared. + * + * @returns `true` if parameters are successfully declared, `false` otherwise. */ + inline bool declare_parameters( const std::string & joint_name, const rclcpp_lifecycle::LifecycleNode::SharedPtr & lifecycle_node) { @@ -164,10 +170,15 @@ inline bool declare_parameters( joint_name, lifecycle_node->get_node_parameters_interface(), lifecycle_node->get_node_logging_interface()); } - -/// Populate a JointLimits instance from the node parameters. /** - * It is assumed that parameter structure is the following: + * @brief Populate a JointLimits instance from the node parameters. + * + * This function reads the joint limit parameters specified in the node's parameters interface and fills + * the provided `JointLimits` instance with the values. It is assumed that the parameter structure follows + * the specified format (as detailed below). If a parameter is not specified in the parameter server, it will + * not be added to the `JointLimits` instance. + * + * Parameter structure: * \code * has_position_limits: bool * min_position: double @@ -182,11 +193,10 @@ inline bool declare_parameters( * max_jerk: double * has_effort_limits: bool * max_effort: double - * angle_wraparound: bool # will be ignored if there are position limits + * angle_wraparound: bool # Will be ignored if position limits are set. * \endcode - * - * Unspecified parameters are not added to the joint limits specification. - * A specification in a yaml would look like this: + * + * An example YAML configuration for the parameters: * \code * * ros__parameters: @@ -207,21 +217,21 @@ inline bool declare_parameters( * max_effort: 20.0 * bar_joint: * has_position_limits: false # Continuous joint - * angle_wraparound: true # available only for continuous joints + * angle_wraparound: true # Available only for continuous joints * has_velocity_limits: true * max_velocity: 4.0 * \endcode - * - * \param[in] joint_name Name of joint whose limits are to be fetched, e.g., "foo_joint". - * \param[in] param_itf node parameters interface of the node where parameters are specified. - * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] limits Where joint limit data gets written into. Limits specified in the parameter - * server will overwrite existing values. Values in \p limits not specified in the parameter server - * remain unchanged. - * - * \returns True if a limits specification is found (i.e., the \p joint_limits/joint_name parameter - * exists in \p node), false otherwise. + * + * @param[in] joint_name Name of the joint whose limits are to be fetched (e.g., "foo_joint"). + * @param[in] param_itf Node parameters interface from which parameters will be fetched. + * @param[in] logging_itf Node logging interface to provide log errors if any. + * @param[out] limits The `JointLimits` instance where the fetched limits will be stored. + * Existing values will be overwritten by the parameters found. + * + * @returns `true` if joint limit specifications are found and populated successfully, + * `false` if no specification is found for the given joint. */ + inline bool get_joint_limits( const std::string & joint_name, const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & param_itf, @@ -754,21 +764,24 @@ inline bool get_joint_limits( joint_name, lifecycle_node->get_node_parameters_interface(), lifecycle_node->get_node_logging_interface(), soft_limits); } - /** - * Check if any of updated parameters are related to SoftJointLimits. + * @brief Check if any updated parameters are related to SoftJointLimits. * - * This method is intended to be used in the parameters update callback. - * It is recommended that it's result is temporarily stored and synchronized with the - * SoftJointLimits structure in the main loop. + * This method is used to check if any parameters that have been updated in the system + * are related to the SoftJointLimits of a specific joint. It is typically called within + * a parameters update callback. The result of this check should be stored temporarily + * and synchronized with the SoftJointLimits structure in the main loop. * - * \param[in] joint_name Name of the joint whose limits should be checked. - * \param[in] parameters List of the parameters that should be checked if they belong to this - * structure and that are used for updating the internal values. - * \param[in] logging_itf node logging interface to provide log errors. - * \param[out] updated_limits structure with updated SoftJointLimits. It is recommended that the - * currently used limits are stored into this structure before calling this method. + * @param[in] joint_name Name of the joint whose limits should be checked (e.g., "foo_joint"). + * @param[in] parameters List of parameters to be checked to determine if they belong + * to the SoftJointLimits structure and are used for updating + * the internal values. + * @param[in] logging_itf Node logging interface to provide log errors if any occur. + * @param[out] updated_limits The structure that will store the updated SoftJointLimits. + * It is recommended that the current limits are stored in this + * structure before calling this method. */ + inline bool check_for_limits_update( const std::string & joint_name, const std::vector & parameters, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & logging_itf, diff --git a/joint_limits/include/joint_limits/joint_limits_urdf.hpp b/joint_limits/include/joint_limits/joint_limits_urdf.hpp index cdcbaf9c9d..4fd7ecc625 100644 --- a/joint_limits/include/joint_limits/joint_limits_urdf.hpp +++ b/joint_limits/include/joint_limits/joint_limits_urdf.hpp @@ -24,11 +24,11 @@ namespace joint_limits { /** - * \brief Populate a JointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will + * @brief Populate a JointLimits instance from URDF joint data. + * @param[in] urdf_joint URDF joint. + * @param[out] limits Where URDF joint limit data gets written into. Limits in \e urdf_joint will * overwrite existing values. Values in \e limits not present in \e urdf_joint remain unchanged. - * \return True if \e urdf_joint has a valid limits specification, false otherwise. + * @return True if \e urdf_joint has a valid limits specification, false otherwise. */ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & limits) { @@ -62,10 +62,10 @@ inline bool getJointLimits(urdf::JointConstSharedPtr urdf_joint, JointLimits & l } /** - * \brief Populate a SoftJointLimits instance from URDF joint data. - * \param[in] urdf_joint URDF joint. - * \param[out] soft_limits Where URDF soft joint limit data gets written into. - * \return True if \e urdf_joint has a valid soft limits specification, false otherwise. + * @brief Populate a SoftJointLimits instance from URDF joint data. + * @param[in] urdf_joint URDF joint. + * @param[out] soft_limits Where URDF soft joint limit data gets written into. + * @return True if \e urdf_joint has a valid soft limits specification, false otherwise. */ inline bool getSoftJointLimits(urdf::JointConstSharedPtr urdf_joint, SoftJointLimits & soft_limits) { diff --git a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp index 107133128c..b4a369cc40 100644 --- a/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp +++ b/transmission_interface/include/transmission_interface/four_bar_linkage_transmission.hpp @@ -128,7 +128,7 @@ class FourBarLinkageTransmission : public Transmission /// Transform variables from actuator to joint space. /** - * \pre Actuator and joint vectors must have size 2 and point to valid data. + * @pre Actuator and joint vectors must have size 2 and point to valid data. * To call this method it is not required that all other data vectors contain valid data, and can * even remain empty. */ diff --git a/transmission_interface/include/transmission_interface/simple_transmission.hpp b/transmission_interface/include/transmission_interface/simple_transmission.hpp index 809472ab75..c12f06a2c4 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission.hpp @@ -84,19 +84,19 @@ class SimpleTransmission : public Transmission { public: /** - * \param[in] joint_to_actuator_reduction Joint to actuator reduction ratio. - * \param[in] joint_offset Joint position offset used in the position mappings. - * \pre Nonzero reduction value. + * @param[in] joint_to_actuator_reduction Joint to actuator reduction ratio. + * @param[in] joint_offset Joint position offset used in the position mappings. + * @pre Nonzero reduction value. */ explicit SimpleTransmission( const double joint_to_actuator_reduction, const double joint_offset = 0.0); /// Set up the data the transmission operates on. /** - * \param[in] joint_handles Vector of interface handles of one joint - * \param[in] actuator_handles Vector of interface handles of one actuator - * \pre Vectors are nonzero. - * \pre Joint handles share the same joint name and actuator handles share the same actuator name. + * @param[in] joint_handles Vector of interface handles of one joint + * @param[in] actuator_handles Vector of interface handles of one actuator + * @pre Vectors are nonzero. + * @pre Joint handles share the same joint name and actuator handles share the same actuator name. */ void configure( const std::vector & joint_handles, diff --git a/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp b/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp index faef32006d..7a3e5b0bd7 100644 --- a/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp +++ b/transmission_interface/include/transmission_interface/simple_transmission_loader.hpp @@ -23,7 +23,7 @@ namespace transmission_interface { /** - * \brief Class for loading a simple transmission instance from configuration data. + * @brief Class for loading a simple transmission instance from configuration data. */ class SimpleTransmissionLoader : public TransmissionLoader { diff --git a/transmission_interface/include/transmission_interface/transmission.hpp b/transmission_interface/include/transmission_interface/transmission.hpp index 6275f37711..11cfd39820 100644 --- a/transmission_interface/include/transmission_interface/transmission.hpp +++ b/transmission_interface/include/transmission_interface/transmission.hpp @@ -22,26 +22,26 @@ namespace transmission_interface { -/// Abstract base class for representing mechanical transmissions. /** + * @brief Abstract base class for representing mechanical transmissions. + * * Mechanical transmissions transform effort/flow variables such that their product (power) remains - * constant. Effort variables for linear and rotational domains are \e force and \e torque; while - * the flow variables are respectively linear velocity and angular velocity. + * constant. In the context of robotics, effort variables for linear and rotational domains are + * \e force and \e torque, while flow variables are \e linear velocity and \e angular velocity. * - * In robotics it is customary to place transmissions between actuators and joints. This interface - * adheres to this naming to identify the input and output spaces of the transformation. The - * provided interfaces allow bidirectional mappings between actuator and joint spaces for effort, - * velocity and position. Position is not a power variable, but the mappings can be implemented - * using the velocity map plus an integration constant representing the offset between actuator and - * joint zeros. + * Typically, transmissions are used to connect actuators and joints in robotic systems. This interface + * is designed to identify the input and output spaces of the transformation, offering bidirectional + * mappings between actuator and joint spaces for effort, velocity, and position. While position itself + * is not a power variable, mappings for position can be derived by using the velocity map and an + * integration constant to represent the offset between actuator and joint zeros. * - * \par Credit - * This interface was inspired by similar existing implementations by PAL Robotics, S.L. and Willow - * Garage Inc. + * @par Credit + * This interface was inspired by similar implementations by PAL Robotics, S.L., and Willow Garage Inc. * - * \note Implementations of this interface must take care of realtime-safety if the code is to be - * run in realtime contexts, as is often the case in robot control. + * @note Implementations of this interface must ensure realtime safety if the code is intended to + * operate in real-time contexts, which is often the case in robot control applications. */ + class Transmission { public: @@ -51,24 +51,32 @@ class Transmission const std::vector & joint_handles, const std::vector & actuator_handles) = 0; - /// Transform \e effort variables from actuator to joint space. - /** - * \param[in] act_data Actuator-space variables. - * \param[out] jnt_data Joint-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the - * number of transmission actuators and joints. Data vectors not used in this map can remain - * empty. - */ + /** + * @brief Transform \e effort variables from actuator to joint space. + * + * This method transforms effort variables, such as torque or force, from actuator-space to joint-space. + * + * @param[in] act_data Actuator-space variables. + * @param[out] jnt_data Joint-space variables. + * + * @pre All non-empty vectors must contain valid data, and their sizes should be consistent with + * the number of transmission actuators and joints. Data vectors not used in this map can remain empty. + */ + virtual void actuator_to_joint() = 0; - /// Transform \e effort variables from joint to actuator space. - /** - * \param[in] jnt_data Joint-space variables. - * \param[out] act_data Actuator-space variables. - * \pre All non-empty vectors must contain valid data and their size should be consistent with the - * number of transmission actuators and joints. Data vectors not used in this map can remain - * empty. - */ + /** + * @brief Transform \e effort variables from joint to actuator space. + * + * This method transforms effort variables, such as torque or force, from joint-space to actuator-space. + * + * @param[in] jnt_data Joint-space variables. + * @param[out] act_data Actuator-space variables. + * + * @pre All non-empty vectors must contain valid data, and their sizes should be consistent with + * the number of transmission actuators and joints. Data vectors not used in this map can remain empty. + */ + virtual void joint_to_actuator() = 0; /** \return Number of actuators managed by transmission, diff --git a/transmission_interface/include/transmission_interface/transmission_loader.hpp b/transmission_interface/include/transmission_interface/transmission_loader.hpp index 962ba90d16..7434d10e23 100644 --- a/transmission_interface/include/transmission_interface/transmission_loader.hpp +++ b/transmission_interface/include/transmission_interface/transmission_loader.hpp @@ -24,7 +24,7 @@ namespace transmission_interface { /** - * \brief Abstract interface for loading transmission instances from configuration data. + * @brief Abstract interface for loading transmission instances from configuration data. * * It also provides convenience methods for specific transmission loaders to leverage. */ From f8ea16d8756a2758f74d6313157d42a1b720f7c4 Mon Sep 17 00:00:00 2001 From: Vishwanath karne <90135551+Vishwanath-Karne@users.noreply.github.com> Date: Mon, 3 Mar 2025 00:50:49 +0530 Subject: [PATCH 2/2] Update hardware_interface/src/component_parser.cpp Co-authored-by: Sai Kishor Kothakota --- hardware_interface/src/component_parser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index ca7ff6678a..684831d9ae 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -219,7 +219,7 @@ std::size_t parse_size_attribute(const tinyxml2::XMLElement * elem) return size; } -//** +/** * @brief Parses the data_type attribute from an XML element. * * This method retrieves the value of the data_type attribute from the given XML element.