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 b74eb1557b..a1423c9668 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" @@ -194,6 +195,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(); @@ -520,6 +530,12 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; +<<<<<<< HEAD +======= + + std::unordered_map> + async_controller_threads_; +>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041)) }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ca93c8fd60..34350d4b69 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -744,6 +744,24 @@ controller_interface::return_type ControllerManager::configure_controller( controller_name.c_str(), new_state.label().c_str()); return controller_interface::return_type::ERROR; } +<<<<<<< HEAD +======= + catch (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while configuring controller '%s'", + controller_name.c_str()); + return controller_interface::return_type::ERROR; + } + + // ASYNCHRONOUS CONTROLLERS: Start background thread for update + if (controller->is_async()) + { + async_controller_threads_.emplace( + controller_name, + std::make_unique(controller, update_rate_)); + } +>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041)) const auto controller_update_rate = controller->get_update_rate(); const auto cm_update_rate = get_update_rate(); @@ -2135,6 +2153,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 new file mode 100644 index 0000000000..73495c92f8 --- /dev/null +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -0,0 +1,122 @@ +// Copyright 2023 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 HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ +#define HARDWARE_INTERFACE__ASYNC_COMPONENTS_HPP_ + +#include +#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 +{ + +class AsyncComponentThread +{ +public: + explicit AsyncComponentThread( + unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + : 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) = delete; + + // Destructor, called when the component is erased from its map. + ~AsyncComponentThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (write_and_read_.joinable()) + { + 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; + + std::visit( + [this](auto & component) + { + 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 (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 (!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); + } + }, + hardware_component_); + } + +private: + std::atomic terminated_{false}; + std::variant hardware_component_; + std::thread write_and_read_{}; + + unsigned int cm_update_rate_; + bool first_iteration = true; + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; +}; + +}; // 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 e94bf5f6ab..5dae905d41 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -369,6 +369,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 e585571d35..e397c5ae21 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -193,6 +193,18 @@ class ResourceStorage hardware.get_name().c_str(), interface.c_str()); } } +<<<<<<< HEAD +======= + + 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); + } +>>>>>>> 2cbe470 (Working async controllers and components [not synchronized] (#1041)) } return result; } @@ -1228,6 +1240,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)