From c0c7dc502f08f31159d536c1341a9981c35421ef Mon Sep 17 00:00:00 2001 From: VX792 Date: Sun, 2 Apr 2023 20:48:50 +0200 Subject: [PATCH] async components --- controller_manager/src/controller_manager.cpp | 6 +- .../hardware_interface/async_components.hpp | 103 +++++++ .../hardware_interface/resource_manager.hpp | 29 +- hardware_interface/src/resource_manager.cpp | 253 +++++++++++++++--- .../test/joint_limits_urdf_test.cpp | 3 +- 5 files changed, 347 insertions(+), 47 deletions(-) create mode 100644 hardware_interface/include/hardware_interface/async_components.hpp diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b912b622e2..ef1c560ad6 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -135,7 +135,8 @@ ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & namespace_, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, namespace_, options), - resource_manager_(std::make_unique()), + resource_manager_(std::make_unique( + this->get_node_clock_interface(), update_rate_)), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -234,6 +235,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript "future anymore. Use hardware_spawner instead."); resource_manager_->activate_all_components(); } + + // At this point all components are supposed to be stored in the resource storage's vector; + resource_manager_->allocate_threads_for_async_components(); } void ControllerManager::init_services() diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp new file mode 100644 index 0000000000..acfbf4d42e --- /dev/null +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -0,0 +1,103 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ +#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ + +#include +#include +#include + +#include "hardware_interface/actuator.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/time.hpp" + +namespace hardware_interface +{ + +template +class AsyncComponentThread +{ +public: + static_assert( + std::is_same::value || + std::is_same::value || + std::is_same::value, + "Async component has to have a valid hardware type."); + + explicit AsyncComponentThread( + HardwareT & component, unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : async_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + { + } + + AsyncComponentThread(const AsyncComponentThread & t) = delete; + AsyncComponentThread(AsyncComponentThread && t) = default; + + ~AsyncComponentThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (write_and_read_.joinable()) + { + write_and_read_.join(); + } + } + + void start() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); } + + void write_and_read() + { + using TimePoint = std::chrono::system_clock::time_point; + + auto previous_time = clock_interface_->get_clock()->now(); + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds())); + + if (async_component_.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto current_time = clock_interface_->get_clock()->now(); + auto measured_period = current_time - previous_time; + previous_time = current_time; + + if constexpr (!std::is_same_v) + { + // write + } + // read + } + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + } + +private: + std::atomic terminated_{false}; + HardwareT & async_component_; + std::thread write_and_read_{}; + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; + unsigned int cm_update_rate_; +}; + +}; // namespace hardware_interface + +#endif // HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1693e85574..1534b97e7e 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -16,25 +16,28 @@ #define HARDWARE_INTERFACE__RESOURCE_MANAGER_HPP_ #include -#include #include #include #include +#include "hardware_interface/actuator.hpp" #include "hardware_interface/hardware_component_info.hpp" #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/loaned_state_interface.hpp" +#include "hardware_interface/sensor.hpp" +#include "hardware_interface/system.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/lifecycle_state_names.hpp" +#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/node.hpp" #include "rclcpp/time.hpp" namespace hardware_interface { -class ActuatorInterface; -class SensorInterface; -class SystemInterface; class ResourceStorage; +class ControllerManager; struct HardwareReadWriteStatus { @@ -46,7 +49,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. - ResourceManager(); + ResourceManager( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr, + int update_rate = 100); /// Constructor for the Resource Manager. /** @@ -65,7 +70,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * "autostart_components" and "autoconfigure_components" instead. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false); + const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -390,6 +396,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_exists(const std::string & key) const; + /// Creates the background threads for all async components. + /** + * The threads get stored in the respective map based on the underlying component's type. + * Even though the callback is registered, and the thread is running by default, the read and write methods + * aren't called until the components get into active state. + */ + void allocate_threads_for_async_components(); + private: void validate_storage(const std::vector & hardware_info) const; @@ -405,6 +419,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; + + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_ = nullptr; + int cm_update_rate_; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 33236afd1c..b24e9f540b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -17,13 +17,16 @@ #include #include #include +#include #include +#include #include #include #include #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" @@ -271,6 +274,20 @@ class ResourceStorage if (result) { + if constexpr (std::is_same_v) + { + async_actuator_threads_.erase(hardware.get_name()); + } + + if constexpr (std::is_same_v) + { + async_system_threads_.erase(hardware.get_name()); + } + + if constexpr (std::is_same_v) + { + async_sensor_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); @@ -489,54 +506,126 @@ class ResourceStorage // TODO(destogl): Propagate "false" up, if happens in initialize_hardware void load_and_initialize_actuator(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, actuators_); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto load_and_init_actuators = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, actuator_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_actuators(async_actuators_); + } + else + { + load_and_init_actuators(actuators_); + } } void load_and_initialize_sensor(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, sensors_); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto load_and_init_sensors = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, sensor_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_sensors(async_sensors_); + } + else + { + load_and_init_sensors(sensors_); + } } void load_and_initialize_system(const HardwareInfo & hardware_info) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, systems_); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto load_and_init_systems = [&](auto & container) + { + check_for_duplicates(hardware_info); + load_hardware(hardware_info, system_loader_, container); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + load_and_init_systems(async_systems_); + } + else + { + load_and_init_systems(systems_); + } } void initialize_actuator( std::unique_ptr actuator, const HardwareInfo & hardware_info) { - this->actuators_.emplace_back(Actuator(std::move(actuator))); - initialize_hardware(hardware_info, actuators_.back()); - import_state_interfaces(actuators_.back()); - import_command_interfaces(actuators_.back()); + auto init_actuators = [&](auto & container) + { + container.emplace_back(Actuator(std::move(actuator))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_actuators(async_actuators_); + } + else + { + init_actuators(actuators_); + } } void initialize_sensor( std::unique_ptr sensor, const HardwareInfo & hardware_info) { - this->sensors_.emplace_back(Sensor(std::move(sensor))); - initialize_hardware(hardware_info, sensors_.back()); - import_state_interfaces(sensors_.back()); + auto init_sensors = [&](auto & container) + { + container.emplace_back(Sensor(std::move(sensor))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_sensors(async_sensors_); + } + else + { + init_sensors(sensors_); + } } void initialize_system( std::unique_ptr system, const HardwareInfo & hardware_info) { - this->systems_.emplace_back(System(std::move(system))); - initialize_hardware(hardware_info, systems_.back()); - import_state_interfaces(systems_.back()); - import_command_interfaces(systems_.back()); + auto init_systems = [&](auto & container) + { + container.emplace_back(System(std::move(system))); + initialize_hardware(hardware_info, container.back()); + import_state_interfaces(container.back()); + import_command_interfaces(container.back()); + }; + + if (hardware_info.is_async) + { + init_systems(async_systems_); + } + else + { + init_systems(systems_); + } } // hardware plugins @@ -548,6 +637,10 @@ 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_; /// Mapping between hardware and controllers that are using it (accessing data from it) @@ -567,15 +660,30 @@ 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_actuator_threads_; + std::unordered_map> + async_system_threads_; + std::unordered_map> + async_sensor_threads_; }; -ResourceManager::ResourceManager() : resource_storage_(std::make_unique()) {} +ResourceManager::ResourceManager( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface, int update_rate) +: resource_storage_(std::make_unique()), + clock_interface_(clock_interface), + cm_update_rate_(update_rate) +{ +} ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool validate_interfaces, bool activate_all) -: resource_storage_(std::make_unique()) + const std::string & urdf, bool validate_interfaces, bool activate_all, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique()), clock_interface_(clock_interface) { load_urdf(urdf, validate_interfaces); @@ -886,18 +994,20 @@ void ResourceManager::import_component( // CM API: Called in "callback/slow"-thread std::unordered_map ResourceManager::get_components_status() { - for (auto & component : resource_storage_->actuators_) + auto loop_and_get_state = [&](auto & container) { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->sensors_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } - for (auto & component : resource_storage_->systems_) - { - resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); - } + for (auto & component : container) + { + resource_storage_->hardware_info_map_[component.get_name()].state = component.get_state(); + } + }; + + 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_; } @@ -1056,6 +1166,24 @@ 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; } @@ -1144,8 +1272,55 @@ bool ResourceManager::state_interface_exists(const std::string & key) const return resource_storage_->state_interface_map_.find(key) != resource_storage_->state_interface_map_.end(); } + // END: "used only in tests and locally" +void ResourceManager::allocate_threads_for_async_components() +{ + for (auto & component : resource_storage_->async_actuators_) + { + if (resource_storage_->hardware_info_map_[component.get_name()].is_async) + { + resource_storage_->async_actuator_threads_.emplace( + std::piecewise_construct, std::forward_as_tuple(component.get_name()), + std::forward_as_tuple(component, cm_update_rate_, clock_interface_)); + } + } + for (auto & component : resource_storage_->async_sensors_) + { + if (resource_storage_->hardware_info_map_[component.get_name()].is_async) + { + resource_storage_->async_sensor_threads_.emplace( + std::piecewise_construct, std::forward_as_tuple(component.get_name()), + std::forward_as_tuple(component, cm_update_rate_, clock_interface_)); + } + } + for (auto & component : resource_storage_->async_systems_) + { + if (resource_storage_->hardware_info_map_[component.get_name()].is_async) + { + resource_storage_->async_system_threads_.emplace( + std::piecewise_construct, std::forward_as_tuple(component.get_name()), + std::forward_as_tuple(component, cm_update_rate_, clock_interface_)); + } + } + + for (auto & thread : resource_storage_->async_actuator_threads_) + { + thread.second.start(); + } + + for (auto & thread : resource_storage_->async_system_threads_) + { + thread.second.start(); + } + + for (auto & thread : resource_storage_->async_sensor_threads_) + { + thread.second.start(); + } +} + // BEGIN: private methods void ResourceManager::validate_storage( diff --git a/joint_limits_interface/test/joint_limits_urdf_test.cpp b/joint_limits_interface/test/joint_limits_urdf_test.cpp index 55effc7117..3914c2195a 100644 --- a/joint_limits_interface/test/joint_limits_urdf_test.cpp +++ b/joint_limits_interface/test/joint_limits_urdf_test.cpp @@ -14,11 +14,12 @@ /// \author Adolfo Rodriguez Tsouroukdissian +#include + #include #include -#include class JointLimitsUrdfTest : public ::testing::Test {