diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 332d565238..d33b167997 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -222,15 +222,6 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; - /// Deletes all async controllers and components. - /** - * Needed to join the threads immediately after the control loop is ended - * to avoid unnecessary iterations. Otherwise - * the threads will be joined only when the controller manager gets destroyed. - */ - CONTROLLER_MANAGER_PUBLIC - void shutdown_async_controllers_and_components(); - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 47ccf17cc8..61b9477e08 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2692,11 +2692,6 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } -void ControllerManager::shutdown_async_controllers_and_components() -{ - resource_manager_->shutdown_async_components(); -} - void ControllerManager::propagate_deactivation_of_chained_mode( const std::vector & controllers) { diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6bff653044..a8f8c079c3 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -147,8 +147,6 @@ int main(int argc, char ** argv) std::this_thread::sleep_until(next_iteration_time); } } - - cm->shutdown_async_controllers_and_components(); }); executor->add_node(cm); diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 9b5360ced0..69c282b051 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -157,6 +157,7 @@ hardware_interface * With (`#1683 `_) the ``rclcpp_lifecycle::State & get_state()`` and ``void set_state(const rclcpp_lifecycle::State & new_state)`` are replaced by ``rclcpp_lifecycle::State & get_lifecycle_state()`` and ``void set_lifecycle_state(const rclcpp_lifecycle::State & new_state)``. This change affects controllers and hardware. This is related to (`#1240 `_) as variant support introduces ``get_state`` and ``set_state`` methods for setting/getting state of handles. * With (`#1421 `_) a key-value storage is added to InterfaceInfo. This allows to define extra params with per Command-/StateInterface in the ``.ros2_control.xacro`` file. * With (`#1763 `_) parsing for SDF published to ``robot_description`` topic is now also supported. +* With (`#1567 `_) all the Hardware components now have a fully functional asynchronous functionality, by simply adding ``is_async`` tag to the ros2_control tag in the URDF. This will allow the hardware components to run in a separate thread, and the controller manager will be able to run the controllers in parallel with the hardware components. joint_limits ************ diff --git a/hardware_interface/CMakeLists.txt b/hardware_interface/CMakeLists.txt index f9637b4f07..4e3fe6a9ae 100644 --- a/hardware_interface/CMakeLists.txt +++ b/hardware_interface/CMakeLists.txt @@ -13,6 +13,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_lifecycle rcpputils rcutils + realtime_tools TinyXML2 tinyxml2_vendor joint_limits diff --git a/hardware_interface/include/hardware_interface/actuator_interface.hpp b/hardware_interface/include/hardware_interface/actuator_interface.hpp index 8aa214e728..6ca478231f 100644 --- a/hardware_interface/include/hardware_interface/actuator_interface.hpp +++ b/hardware_interface/include/hardware_interface/actuator_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,30 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod clock_interface_ = clock_interface; actuator_logger_ = logger.get_child("hardware_component.actuator." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -321,6 +346,50 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod 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. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -333,6 +402,49 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod */ 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. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -426,6 +538,7 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod std::vector unlisted_commands_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; @@ -433,6 +546,12 @@ class ActuatorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNod // interface names to Handle accessed through getters/setters std::unordered_map actuator_states_; std::unordered_map actuator_commands_; + enum class TriggerType + { + READ, + WRITE + }; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/hardware_info.hpp b/hardware_interface/include/hardware_interface/hardware_info.hpp index f62329ee62..9d96190954 100644 --- a/hardware_interface/include/hardware_interface/hardware_info.hpp +++ b/hardware_interface/include/hardware_interface/hardware_info.hpp @@ -178,6 +178,8 @@ struct HardwareInfo unsigned int rw_rate; /// Component is async bool is_async; + /// Async thread priority + int thread_priority; /// 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/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5592fc492..cdde7550d3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -426,12 +426,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager return_type set_component_state( const std::string & component_name, rclcpp_lifecycle::State & target_state); - /// Deletes all async components from the resource storage - /** - * Needed to join the threads immediately after the control loop is ended. - */ - void shutdown_async_components(); - /// Reads all loaded hardware components. /** * Reads from all active hardware components. diff --git a/hardware_interface/include/hardware_interface/sensor_interface.hpp b/hardware_interface/include/hardware_interface/sensor_interface.hpp index 455f6216a2..93de2a6494 100644 --- a/hardware_interface/include/hardware_interface/sensor_interface.hpp +++ b/hardware_interface/include/hardware_interface/sensor_interface.hpp @@ -34,6 +34,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -111,6 +112,16 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; sensor_logger_ = logger.get_child("hardware_component.sensor." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + read_async_handler_ = std::make_unique>(); + read_async_handler_->init( + std::bind(&SensorInterface::read, this, std::placeholders::_1, std::placeholders::_2), + info_.thread_priority); + read_async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -214,6 +225,40 @@ 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. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + std::tie(trigger_status, result) = read_async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while read async handler is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -294,6 +339,7 @@ class SensorInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::vector unlisted_states_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> read_async_handler_; private: rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; diff --git a/hardware_interface/include/hardware_interface/system_interface.hpp b/hardware_interface/include/hardware_interface/system_interface.hpp index 32d0b8a48a..ec649b0225 100644 --- a/hardware_interface/include/hardware_interface/system_interface.hpp +++ b/hardware_interface/include/hardware_interface/system_interface.hpp @@ -36,6 +36,7 @@ #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/async_function_handler.hpp" namespace hardware_interface { @@ -114,6 +115,30 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI clock_interface_ = clock_interface; system_logger_ = logger.get_child("hardware_component.system." + hardware_info.name); info_ = hardware_info; + if (info_.is_async) + { + RCLCPP_INFO_STREAM( + get_logger(), "Starting async handler with scheduler priority: " << info_.thread_priority); + async_handler_ = std::make_unique>(); + async_handler_->init( + [this](const rclcpp::Time & time, const rclcpp::Duration & period) + { + if (next_trigger_ == TriggerType::READ) + { + const auto ret = read(time, period); + next_trigger_ = TriggerType::WRITE; + return ret; + } + else + { + const auto ret = write(time, period); + next_trigger_ = TriggerType::READ; + return ret; + } + }, + info_.thread_priority); + async_handler_->start_thread(); + } return on_init(hardware_info); }; @@ -350,6 +375,50 @@ 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. + */ + return_type trigger_read(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::WRITE) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async handler call is still pending for hardware " + "interface : '%s'. Skipping read cycle and will wait for a write cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger read called while write async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger read cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = read(time, period); + } + return result; + } + /// Read the current state values from the actuator. /** * The data readings from the physical hardware has to be updated @@ -362,6 +431,49 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI */ 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. + */ + return_type trigger_write(const rclcpp::Time & time, const rclcpp::Duration & period) + { + return_type result = return_type::ERROR; + if (info_.is_async) + { + bool trigger_status = true; + if (next_trigger_ == TriggerType::READ) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async handler call is still pending for hardware " + "interface : '%s'. Skipping write cycle and will wait for a read cycle!", + info_.name.c_str()); + return return_type::OK; + } + std::tie(trigger_status, result) = async_handler_->trigger_async_callback(time, period); + if (!trigger_status) + { + RCLCPP_WARN( + get_logger(), + "Trigger write called while read async trigger is still in progress for hardware " + "interface : '%s'. Failed to trigger write cycle!", + info_.name.c_str()); + return return_type::OK; + } + } + else + { + result = write(time, period); + } + return result; + } + /// Write the current command values to the actuator. /** * The physical hardware shall be updated with the latest value from @@ -453,6 +565,7 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI std::unordered_map unlisted_command_interfaces_; rclcpp_lifecycle::State lifecycle_state_; + std::unique_ptr> async_handler_; // Exported Command- and StateInterfaces in order they are listed in the hardware description. std::vector joint_states_; @@ -472,6 +585,12 @@ class SystemInterface : public rclcpp_lifecycle::node_interfaces::LifecycleNodeI // interface names to Handle accessed through getters/setters std::unordered_map system_states_; std::unordered_map system_commands_; + enum class TriggerType + { + READ, + WRITE + }; + std::atomic next_trigger_ = TriggerType::READ; }; } // namespace hardware_interface diff --git a/hardware_interface/package.xml b/hardware_interface/package.xml index 23b882ab02..c70ae6fc8b 100644 --- a/hardware_interface/package.xml +++ b/hardware_interface/package.xml @@ -16,6 +16,7 @@ pluginlib rclcpp_lifecycle rcpputils + realtime_tools tinyxml2_vendor joint_limits urdf diff --git a/hardware_interface/src/actuator.cpp b/hardware_interface/src/actuator.cpp index 0e4ae95ca9..81bebf1182 100644 --- a/hardware_interface/src/actuator.cpp +++ b/hardware_interface/src/actuator.cpp @@ -302,7 +302,7 @@ return_type Actuator::read(const rclcpp::Time & time, const rclcpp::Duration & p impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -324,7 +324,7 @@ return_type Actuator::write(const rclcpp::Time & time, const rclcpp::Duration & impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/component_parser.cpp b/hardware_interface/src/component_parser.cpp index 146a5aef05..a3e9efaa3a 100644 --- a/hardware_interface/src/component_parser.cpp +++ b/hardware_interface/src/component_parser.cpp @@ -60,6 +60,7 @@ constexpr const auto kReductionAttribute = "mechanical_reduction"; constexpr const auto kOffsetAttribute = "offset"; constexpr const auto kReadWriteRateAttribute = "rw_rate"; constexpr const auto kIsAsyncAttribute = "is_async"; +constexpr const auto kThreadPriorityAttribute = "thread_priority"; } // namespace @@ -273,6 +274,35 @@ bool parse_is_async_attribute(const tinyxml2::XMLElement * elem) 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. + * + * \param[in] elem XMLElement that has the thread_priority attribute. + * \return positive integer specifying the thread priority. + */ +int parse_thread_priority_attribute(const tinyxml2::XMLElement * elem) +{ + const tinyxml2::XMLAttribute * attr = elem->FindAttribute(kThreadPriorityAttribute); + if (!attr) + { + return 50; + } + std::string s = attr->Value(); + std::regex int_re("[1-9][0-9]*"); + if (std::regex_match(s, int_re)) + { + return std::stoi(s); + } + else + { + throw std::runtime_error( + "Could not parse thread_priority tag in \"" + std::string(elem->Name()) + "\"." + "Got \"" + + s + "\", but expected a non-zero positive integer."); + } +} + /// Search XML snippet from URDF for parameters. /** * \param[in] params_it pointer to the iterator where parameters info should be found @@ -614,6 +644,8 @@ HardwareInfo parse_resource_from_xml( hardware.type = get_attribute_value(ros2_control_it, kTypeAttribute, kROS2ControlTag); hardware.rw_rate = parse_rw_rate_attribute(ros2_control_it); hardware.is_async = parse_is_async_attribute(ros2_control_it); + hardware.thread_priority = hardware.is_async ? parse_thread_priority_attribute(ros2_control_it) + : std::numeric_limits::max(); // 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 e77917af9d..0d9ad51eac 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -26,7 +26,6 @@ #include "hardware_interface/actuator.hpp" #include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/async_components.hpp" #include "hardware_interface/component_parser.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/sensor.hpp" @@ -302,15 +301,6 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } - - if (hardware_info_map_[hardware.get_name()].is_async) - { - async_component_threads_.emplace( - std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), - std::forward_as_tuple(cm_update_rate_, clock_interface_)); - - async_component_threads_.at(hardware.get_name()).register_component(&hardware); - } } if (!hardware.get_group_name().empty()) { @@ -431,7 +421,6 @@ class ResourceStorage if (result) { remove_all_hardware_interfaces_from_available_list(hardware.get_name()); - async_component_threads_.erase(hardware.get_name()); // TODO(destogl): change this - deimport all things if there is there are interfaces there // deimport_non_movement_command_interfaces(hardware); // deimport_state_interfaces(hardware); @@ -466,16 +455,6 @@ class ResourceStorage get_logger(), "Unknown exception occurred while activating hardware '%s'", hardware.get_name().c_str()); } - - if (result) - { - if (async_component_threads_.find(hardware.get_name()) != async_component_threads_.end()) - { - async_component_threads_.at(hardware.get_name()).activate(); - } - // TODO(destogl): make all command interfaces available (currently are all available) - } - return result; } @@ -827,15 +806,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_actuators(async_actuators_); - } - else - { - return load_and_init_actuators(actuators_); - } + return load_and_init_actuators(actuators_); } bool load_and_initialize_sensor(const HardwareInfo & hardware_info) @@ -860,14 +831,7 @@ class ResourceStorage return true; }; - if (hardware_info.is_async) - { - return load_and_init_sensors(async_sensors_); - } - else - { - return load_and_init_sensors(sensors_); - } + return load_and_init_sensors(sensors_); } bool load_and_initialize_system(const HardwareInfo & hardware_info) @@ -892,15 +856,7 @@ class ResourceStorage } return true; }; - - if (hardware_info.is_async) - { - return load_and_init_systems(async_systems_); - } - else - { - return load_and_init_systems(systems_); - } + return load_and_init_systems(systems_); } void initialize_actuator( @@ -922,14 +878,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_actuators(async_actuators_); - } - else - { - init_actuators(actuators_); - } + init_actuators(actuators_); } void initialize_sensor( @@ -950,14 +899,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_sensors(async_sensors_); - } - else - { - init_sensors(sensors_); - } + init_sensors(sensors_); } void initialize_system( @@ -979,14 +921,7 @@ class ResourceStorage } }; - if (hardware_info.is_async) - { - init_systems(async_systems_); - } - else - { - init_systems(systems_); - } + init_systems(systems_); } void clear() @@ -995,10 +930,6 @@ class ResourceStorage sensors_.clear(); systems_.clear(); - async_actuators_.clear(); - async_sensors_.clear(); - async_systems_.clear(); - hardware_info_map_.clear(); state_interface_map_.clear(); command_interface_map_.clear(); @@ -1056,10 +987,6 @@ class ResourceStorage std::vector sensors_; std::vector systems_; - std::vector async_actuators_; - std::vector async_sensors_; - std::vector async_systems_; - std::unordered_map hardware_info_map_; std::unordered_map hw_group_state_; @@ -1083,9 +1010,6 @@ class ResourceStorage /// List of all claimed command interfaces std::unordered_map claimed_command_interface_map_; - /// List of async components by type - std::unordered_map async_component_threads_; - // Update rate of the controller manager, and the clock interface of its node // Used by async components. unsigned int cm_update_rate_ = 100; @@ -1527,11 +1451,8 @@ std::unordered_map ResourceManager::get_comp }; loop_and_get_state(resource_storage_->actuators_); - loop_and_get_state(resource_storage_->async_actuators_); loop_and_get_state(resource_storage_->sensors_); - loop_and_get_state(resource_storage_->async_sensors_); loop_and_get_state(resource_storage_->systems_); - loop_and_get_state(resource_storage_->async_systems_); return resource_storage_->hardware_info_map_; } @@ -1799,35 +1720,10 @@ return_type ResourceManager::set_component_state( std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), resource_storage_->systems_); } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_actuators_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_systems_); - } - if (!found) - { - found = find_set_component_state( - std::bind(&ResourceStorage::set_component_state, resource_storage_.get(), _1, _2), - resource_storage_->async_sensors_); - } return result; } -void ResourceManager::shutdown_async_components() -{ - resource_storage_->async_component_threads_.erase( - resource_storage_->async_component_threads_.begin(), - resource_storage_->async_component_threads_.end()); -} - // CM API: Called in "update"-thread HardwareReadWriteStatus ResourceManager::read( const rclcpp::Time & time, const rclcpp::Duration & period) diff --git a/hardware_interface/src/sensor.cpp b/hardware_interface/src/sensor.cpp index ff193ff8e1..ae4d930bbe 100644 --- a/hardware_interface/src/sensor.cpp +++ b/hardware_interface/src/sensor.cpp @@ -256,7 +256,7 @@ return_type Sensor::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/src/system.cpp b/hardware_interface/src/system.cpp index 9a27aa4f68..89d6afd42e 100644 --- a/hardware_interface/src/system.cpp +++ b/hardware_interface/src/system.cpp @@ -300,7 +300,7 @@ return_type System::read(const rclcpp::Time & time, const rclcpp::Duration & per impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->read(time, period); + result = impl_->trigger_read(time, period); if (result == return_type::ERROR) { error(); @@ -322,7 +322,7 @@ return_type System::write(const rclcpp::Time & time, const rclcpp::Duration & pe impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || impl_->get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - result = impl_->write(time, period); + result = impl_->trigger_write(time, period); if (result == return_type::ERROR) { error(); diff --git a/hardware_interface/test/test_component_parser.cpp b/hardware_interface/test/test_component_parser.cpp index 8c535f04a9..05fe930e25 100644 --- a/hardware_interface/test/test_component_parser.cpp +++ b/hardware_interface/test/test_component_parser.cpp @@ -879,6 +879,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_robot_with_gpio EXPECT_EQ( hardware_info.hardware_plugin_name, "ros2_control_demo_hardware/RRBotSystemWithGPIOHardware"); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); ASSERT_THAT(hardware_info.joints, SizeIs(2)); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); @@ -949,6 +951,8 @@ TEST_F(TestComponentParser, successfully_parse_valid_urdf_system_with_size_and_d ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_FALSE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, std::numeric_limits::max()); EXPECT_EQ(hardware_info.joints[0].name, "joint1"); EXPECT_EQ(hardware_info.joints[0].type, "joint"); EXPECT_THAT(hardware_info.joints[0].command_interfaces, SizeIs(1)); @@ -1337,6 +1341,59 @@ TEST_F(TestComponentParser, successfully_parse_urdf_system_continuous_with_limit EXPECT_FALSE(hardware_info.limits.at("joint2").has_jerk_limits); } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_components) +{ + std::string urdf_to_test = ros2_control_test_assets::minimal_async_robot_urdf; + const auto control_hardware = parse_control_resources_from_urdf(urdf_to_test); + ASSERT_THAT(control_hardware, SizeIs(3)); + auto hardware_info = control_hardware[0]; + + // Actuator + EXPECT_EQ(hardware_info.name, "TestActuatorHardware"); + EXPECT_EQ(hardware_info.type, "actuator"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 30); + + EXPECT_EQ(hardware_info.joints[0].name, "joint1"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + // Sensor + hardware_info = control_hardware[1]; + EXPECT_EQ(hardware_info.name, "TestSensorHardware"); + EXPECT_EQ(hardware_info.type, "sensor"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, IsEmpty()); + ASSERT_THAT(hardware_info.sensors, SizeIs(1)); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 50); + + EXPECT_EQ(hardware_info.sensors[0].name, "sensor1"); + EXPECT_EQ(hardware_info.sensors[0].type, "sensor"); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces, SizeIs(1)); + EXPECT_THAT(hardware_info.sensors[0].command_interfaces, IsEmpty()); + EXPECT_THAT(hardware_info.sensors[0].state_interfaces[0].name, "velocity"); + + // System + hardware_info = control_hardware[2]; + EXPECT_EQ(hardware_info.name, "TestSystemHardware"); + EXPECT_EQ(hardware_info.type, "system"); + ASSERT_THAT(hardware_info.group, IsEmpty()); + ASSERT_THAT(hardware_info.joints, SizeIs(2)); + ASSERT_THAT(hardware_info.gpios, SizeIs(1)); + + EXPECT_EQ(hardware_info.joints[0].name, "joint2"); + EXPECT_EQ(hardware_info.joints[0].type, "joint"); + + EXPECT_EQ(hardware_info.joints[1].name, "joint3"); + EXPECT_EQ(hardware_info.joints[1].type, "joint"); + EXPECT_EQ(hardware_info.gpios[0].name, "configuration"); + EXPECT_EQ(hardware_info.gpios[0].type, "gpio"); + ASSERT_TRUE(hardware_info.is_async); + ASSERT_EQ(hardware_info.thread_priority, 70); +} + TEST_F(TestComponentParser, successfully_parse_parameter_empty) { const std::string urdf_to_test = @@ -1377,6 +1434,16 @@ TEST_F(TestComponentParser, successfully_parse_parameter_empty) } } +TEST_F(TestComponentParser, successfully_parse_valid_urdf_async_invalid_thread_priority) +{ + std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + + "" + + std::string(ros2_control_test_assets::urdf_tail); + ; + ASSERT_THROW(parse_control_resources_from_urdf(urdf_to_test), std::runtime_error); +} + TEST_F(TestComponentParser, negative_size_throws_error) { std::string urdf_to_test = std::string(ros2_control_test_assets::urdf_head) + diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5f7640546a..be9e672b3b 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -1958,6 +1958,206 @@ TEST_F( check_read_and_write_cycles(false); } +class ResourceManagerTestAsyncReadWrite : public ResourceManagerTest +{ +public: + void setup_resource_manager_and_do_initial_checks() + { + const auto minimal_robot_urdf_async = + std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::async_hardware_resources) + + std::string(ros2_control_test_assets::urdf_tail); + rm = std::make_shared(node_, minimal_robot_urdf_async, false); + activate_components(*rm); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + + // Check that all the components are async + ASSERT_TRUE(status_map[TEST_ACTUATOR_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SYSTEM_HARDWARE_NAME].is_async); + ASSERT_TRUE(status_map[TEST_SENSOR_HARDWARE_NAME].is_async); + + claimed_itfs.push_back( + rm->claim_command_interface(TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES[0])); + claimed_itfs.push_back(rm->claim_command_interface(TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES[0])); + + state_itfs.push_back(rm->claim_state_interface(TEST_ACTUATOR_HARDWARE_STATE_INTERFACES[1])); + state_itfs.push_back(rm->claim_state_interface(TEST_SYSTEM_HARDWARE_STATE_INTERFACES[1])); + + check_if_interface_available(true, true); + // with default values read and write should run without any problems + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + { + // claimed_itfs[0].set_value(10.0); + // claimed_itfs[1].set_value(20.0); + auto [ok, failed_hardware_names] = rm->write(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + } + time = time + duration; + check_if_interface_available(true, true); + } + + // check if all interfaces are available + void check_if_interface_available(const bool actuator_interfaces, const bool system_interfaces) + { + for (const auto & interface : TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), actuator_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + EXPECT_EQ(rm->command_interface_is_available(interface), system_interfaces); + } + for (const auto & interface : TEST_SYSTEM_HARDWARE_STATE_INTERFACES) + { + EXPECT_EQ(rm->state_interface_is_available(interface), system_interfaces); + } + }; + + void check_read_and_write_cycles(bool check_for_updated_values) + { + double prev_act_state_value = state_itfs[0].get_value(); + double prev_system_state_value = state_itfs[1].get_value(); + const double actuator_increment = 10.0; + const double system_increment = 20.0; + for (size_t i = 1; i < 100; i++) + { + auto [ok, failed_hardware_names] = rm->read(time, duration); + EXPECT_TRUE(ok); + EXPECT_TRUE(failed_hardware_names.empty()); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + // The values are computations exactly within the test_components + if (check_for_updated_values) + { + prev_system_state_value = claimed_itfs[1].get_value() / 2.0; + prev_act_state_value = claimed_itfs[0].get_value() / 2.0; + } + claimed_itfs[0].set_value(claimed_itfs[0].get_value() + actuator_increment); + claimed_itfs[1].set_value(claimed_itfs[1].get_value() + system_increment); + // This is needed to account for any missing hits to the read and write cycles as the tests + // are going to be run on a non-RT operating system + ASSERT_NEAR(state_itfs[0].get_value(), prev_act_state_value, actuator_increment / 2.0); + ASSERT_NEAR(state_itfs[1].get_value(), prev_system_state_value, system_increment / 2.0); + auto [ok_write, failed_hardware_names_write] = rm->write(time, duration); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + EXPECT_TRUE(ok_write); + EXPECT_TRUE(failed_hardware_names_write.empty()); + time = time + duration; + } + } + +public: + std::shared_ptr rm; + unsigned int actuator_rw_rate_, system_rw_rate_, cm_update_rate_; + std::vector claimed_itfs; + std::vector state_itfs; + + rclcpp::Time time = rclcpp::Time(1657232, 0); + const rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.01); +}; + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_activate) +{ + setup_resource_manager_and_do_initial_checks(); + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_deactivate) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_inactive( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + hardware_interface::lifecycle_state_names::INACTIVE); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_inactive); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_inactive); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + check_read_and_write_cycles(true); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_unconfigured) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_unconfigured( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + hardware_interface::lifecycle_state_names::UNCONFIGURED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_unconfigured); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_unconfigured); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + + check_read_and_write_cycles(false); +} + +TEST_F(ResourceManagerTestAsyncReadWrite, test_components_with_async_components_on_finalized) +{ + setup_resource_manager_and_do_initial_checks(); + + // Now deactivate all the components and test the same as above + rclcpp_lifecycle::State state_finalized( + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, + hardware_interface::lifecycle_state_names::FINALIZED); + rm->set_component_state(TEST_SYSTEM_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_ACTUATOR_HARDWARE_NAME, state_finalized); + rm->set_component_state(TEST_SENSOR_HARDWARE_NAME, state_finalized); + + auto status_map = rm->get_components_status(); + EXPECT_EQ( + status_map[TEST_ACTUATOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SYSTEM_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + EXPECT_EQ( + status_map[TEST_SENSOR_HARDWARE_NAME].state.id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED); + + check_read_and_write_cycles(false); +} + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 5f4512a9d1..1744a806bc 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -662,7 +662,7 @@ const auto hardware_resources = const auto async_hardware_resources = R"( - + test_actuator @@ -683,7 +683,7 @@ const auto async_hardware_resources = - + test_system 2