diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp new file mode 100644 index 0000000000..5601ff4703 --- /dev/null +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -0,0 +1,111 @@ +// Copyright 2024 ros2_control development team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ +#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ + +#include +#include +#include + +#include "controller_interface_base.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +namespace controller_interface +{ + +class AsyncControllerThread +{ +public: + /// Constructor for the AsyncControllerThread object. + /** + * + * \param[in] controller shared pointer to a controller. + * \param[in] cm_update_rate the controller manager's update rate. + */ + AsyncControllerThread( + std::shared_ptr & controller, int cm_update_rate) + : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) + { + } + + AsyncControllerThread(const AsyncControllerThread & t) = delete; + AsyncControllerThread(AsyncControllerThread && t) = delete; + + // Destructor, called when the component is erased from its map. + ~AsyncControllerThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (thread_.joinable()) + { + thread_.join(); + } + } + + /// Creates the controller's thread. + /** + * Called when the controller is activated. + * + */ + void activate() + { + thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); + } + + /// Periodically execute the controller's update method. + /** + * Callback of the async controller's thread. + * **Not synchronized with the controller manager's write and read currently** + * + */ + void controller_update_callback() + { + using TimePoint = std::chrono::system_clock::time_point; + unsigned int used_update_rate = + controller_->get_update_rate() == 0 ? cm_update_rate_ : controller_->get_update_rate(); + + auto previous_time = controller_->get_node()->now(); + while (!terminated_.load(std::memory_order_relaxed)) + { + auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); + TimePoint next_iteration_time = + TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); + + if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + auto const current_time = controller_->get_node()->now(); + auto const measured_period = current_time - previous_time; + previous_time = current_time; + controller_->update( + controller_->get_node()->now(), + (controller_->get_update_rate() != cm_update_rate_ && controller_->get_update_rate() != 0) + ? rclcpp::Duration::from_seconds(1.0 / controller_->get_update_rate()) + : measured_period); + } + + next_iteration_time += period; + std::this_thread::sleep_until(next_iteration_time); + } + } + +private: + std::atomic terminated_; + std::shared_ptr controller_; + std::thread thread_; + unsigned int cm_update_rate_; +}; + +} // namespace controller_interface + +#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 532e8909c5..a7e1c45de0 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -23,6 +23,7 @@ #include #include +#include "controller_interface/async_controller.hpp" #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" @@ -196,6 +197,15 @@ 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(); @@ -560,65 +570,7 @@ class ControllerManager : public rclcpp::Node SwitchParams switch_params_; - class ControllerThreadWrapper - { - public: - ControllerThreadWrapper( - std::shared_ptr & controller, - int cm_update_rate) - : terminated_(false), controller_(controller), thread_{}, cm_update_rate_(cm_update_rate) - { - } - - ControllerThreadWrapper(const ControllerThreadWrapper & t) = delete; - ControllerThreadWrapper(ControllerThreadWrapper && t) = default; - ~ControllerThreadWrapper() - { - terminated_.store(true, std::memory_order_seq_cst); - if (thread_.joinable()) - { - thread_.join(); - } - } - - void activate() - { - thread_ = std::thread(&ControllerThreadWrapper::call_controller_update, this); - } - - void call_controller_update() - { - using TimePoint = std::chrono::system_clock::time_point; - unsigned int used_update_rate = - controller_->get_update_rate() == 0 - ? cm_update_rate_ - : controller_ - ->get_update_rate(); // determines if the controller's or CM's update rate is used - - while (!terminated_.load(std::memory_order_relaxed)) - { - auto const period = std::chrono::nanoseconds(1'000'000'000 / used_update_rate); - TimePoint next_iteration_time = - TimePoint(std::chrono::nanoseconds(controller_->get_node()->now().nanoseconds())); - - if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - // critical section, not implemented yet - } - - next_iteration_time += period; - std::this_thread::sleep_until(next_iteration_time); - } - } - - private: - std::atomic terminated_; - std::shared_ptr controller_; - std::thread thread_; - unsigned int cm_update_rate_; - }; - - std::unordered_map> + std::unordered_map> async_controller_threads_; }; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2db5e1ab79..cabf731e6f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -765,7 +765,8 @@ controller_interface::return_type ControllerManager::configure_controller( if (controller->is_async()) { async_controller_threads_.emplace( - controller_name, std::make_unique(controller, update_rate_)); + controller_name, + std::make_unique(controller, update_rate_)); } const auto controller_update_rate = controller->get_update_rate(); @@ -2234,6 +2235,13 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } +void ControllerManager::shutdown_async_controllers_and_components() +{ + async_controller_threads_.erase( + async_controller_threads_.begin(), async_controller_threads_.end()); + 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 e7f88f2c20..9f8db4f3d9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -85,6 +85,8 @@ int main(int argc, char ** argv) next_iteration_time += period; std::this_thread::sleep_until(next_iteration_time); } + + cm->shutdown_async_controllers_and_components(); }); executor->add_node(cm); diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index 2fa6fd7f85..73495c92f8 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "hardware_interface/actuator.hpp" @@ -34,29 +35,23 @@ class AsyncComponentThread { public: explicit AsyncComponentThread( - Actuator * component, unsigned int update_rate, + unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + : cm_update_rate_(update_rate), clock_interface_(clock_interface) { } - explicit AsyncComponentThread( - System * component, unsigned int update_rate, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) - { - } - - explicit AsyncComponentThread( - Sensor * component, unsigned int update_rate, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) - : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface) + // Fills the internal variant with the desired component. + template + void register_component(T * component) { + hardware_component_ = component; } AsyncComponentThread(const AsyncComponentThread & t) = delete; - AsyncComponentThread(AsyncComponentThread && t) = default; + AsyncComponentThread(AsyncComponentThread && t) = delete; + // Destructor, called when the component is erased from its map. ~AsyncComponentThread() { terminated_.store(true, std::memory_order_seq_cst); @@ -65,9 +60,19 @@ class AsyncComponentThread write_and_read_.join(); } } - + /// Creates the component's thread. + /** + * Called when the component is activated. + * + */ void activate() { write_and_read_ = std::thread(&AsyncComponentThread::write_and_read, this); } + /// Periodically execute the component's write and read methods. + /** + * Callback of the async component's thread. + * **Not synchronized with the controller manager's update currently** + * + */ void write_and_read() { using TimePoint = std::chrono::system_clock::time_point; @@ -88,8 +93,12 @@ class AsyncComponentThread auto measured_period = current_time - previous_time; previous_time = current_time; - // write - // read + if (!first_iteration) + { + component->write(clock_interface_->get_clock()->now(), measured_period); + } + component->read(clock_interface_->get_clock()->now(), measured_period); + first_iteration = false; } next_iteration_time += period; std::this_thread::sleep_until(next_iteration_time); @@ -104,6 +113,7 @@ class AsyncComponentThread std::thread write_and_read_{}; unsigned int cm_update_rate_; + bool first_iteration = true; rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; }; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..91a98d1e96 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -376,6 +376,12 @@ 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/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..d23965a65f 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -206,7 +206,9 @@ class ResourceStorage { async_component_threads_.emplace( std::piecewise_construct, std::forward_as_tuple(hardware.get_name()), - std::forward_as_tuple(&hardware, cm_update_rate_, clock_interface_)); + std::forward_as_tuple(cm_update_rate_, clock_interface_)); + + async_component_threads_.at(hardware.get_name()).register_component(&hardware); } } return result; @@ -1324,6 +1326,13 @@ return_type ResourceManager::set_component_state( 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)