diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 71e4409d8b..fdaffac06f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -153,6 +153,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_INTERFACE_PUBLIC + bool is_async() const; + /// Declare and initialize a parameter with a type. /** * @@ -220,6 +223,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::vector command_interfaces_; std::vector state_interfaces_; unsigned int update_rate_ = 0; + bool is_async_ = false; private: std::shared_ptr node_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 035d2572e1..6b4d556c48 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -34,6 +34,7 @@ return_type ControllerInterfaceBase::init( try { auto_declare("update_rate", 0); + auto_declare("is_async", false); } catch (const std::exception & e) { @@ -84,6 +85,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { update_rate_ = get_node()->get_parameter("update_rate").as_int(); + is_async_ = get_node()->get_parameter("is_async").as_bool(); } return get_node()->configure(); diff --git a/hardware_interface/include/hardware_interface/hardware_component_info.hpp b/hardware_interface/include/hardware_interface/hardware_component_info.hpp index cd94c67fcb..45afebdb34 100644 --- a/hardware_interface/include/hardware_interface/hardware_component_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_component_info.hpp @@ -42,6 +42,9 @@ struct HardwareComponentInfo /// Component pluginlib plugin name. std::string plugin_name; + /// Component is async + bool is_async; + /// Component current state. rclcpp_lifecycle::State state; diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index 073d197f9a..aed5e1d2b8 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -54,6 +54,7 @@ struct ComponentInfo std::string name; /// Type of the component: sensor, joint, or GPIO. std::string type; + /** * Name of the command interfaces that can be set, e.g. "position", "velocity", etc. * Used by joints and GPIOs. @@ -106,6 +107,8 @@ struct HardwareInfo std::string name; /// Type of the hardware: actuator, sensor or system. std::string type; + /// Component is async + bool is_async; /// Name of the pluginlib plugin of the hardware that will be loaded. std::string hardware_plugin_name; /// (Optional) Key-value pairs for hardware parameters. diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index cdd4206a04..861800ad00 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -47,6 +47,8 @@ constexpr const auto kTypeAttribute = "type"; constexpr const auto kRoleAttribute = "role"; constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; +constexpr const auto kIsAsyncAttribute = "is_async"; + } // namespace namespace hardware_interface @@ -211,6 +213,20 @@ std::string parse_data_type_attribute(const tinyxml2::XMLElement * elem) return data_type; } +/// Parse is_async attribute +/** + * Parses an XMLElement and returns the value of the is_async attribute. + * Defaults to "false" if not specified. + * + * \param[in] elem XMLElement that has the data_type attribute. + * \return boolean specifying the if 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 ? strcasecmp(attr->Value(), "true") == 0 : false; +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -499,6 +515,7 @@ HardwareInfo parse_resource_from_xml( HardwareInfo hardware; hardware.name = get_attribute_value(ros2_control_it, kNameAttribute, kROS2ControlTag); hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); + hardware.is_async = parse_is_async_attribute(ros2_control_it); // Parse everything under ros2_control tag hardware.hardware_plugin_name = ""; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7e88cca9b5..fd874c806c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -97,6 +97,7 @@ class ResourceStorage component_info.name = hardware_info.name; component_info.type = hardware_info.type; component_info.plugin_name = hardware_info.hardware_plugin_name; + component_info.is_async = hardware_info.is_async; hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert(