From 43e36c31e49ae8dff047b92fc0bb62a0de44a007 Mon Sep 17 00:00:00 2001 From: VX792 Date: Mon, 29 May 2023 12:09:45 +0200 Subject: [PATCH 01/11] add read + write, move async controllers --- .../controller_interface/async_controller.hpp | 78 +++++++++++++++++++ .../controller_manager/controller_manager.hpp | 68 +++------------- controller_manager/src/controller_manager.cpp | 8 +- controller_manager/src/ros2_control_node.cpp | 3 + .../hardware_interface/async_components.hpp | 24 ++---- .../hardware_interface/resource_manager.hpp | 3 + hardware_interface/src/resource_manager.cpp | 15 +++- 7 files changed, 122 insertions(+), 77 deletions(-) create mode 100644 controller_interface/include/controller_interface/async_controller.hpp 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..181b0c1ac3 --- /dev/null +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -0,0 +1,78 @@ +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "controller_interface_base.hpp" + +namespace controller_interface +{ + +class AsyncControllerThread + { + public: + 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) = default; + ~AsyncControllerThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (thread_.joinable()) + { + thread_.join(); + } + } + + void activate() + { + thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); + } + + 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(); // determines if the controller's or CM's update rate is used + + rclcpp::Time 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_; + }; + +} \ No newline at end of file diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 537f0447be..8cdc2148c3 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -26,6 +26,8 @@ #include "controller_interface/chainable_controller_interface.hpp" #include "controller_interface/controller_interface.hpp" #include "controller_interface/controller_interface_base.hpp" +#include "controller_interface/async_controller.hpp" + #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" @@ -190,6 +192,12 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; + CONTROLLER_MANAGER_PUBLIC + void shutdown_async_controllers_and_components(); + + CONTROLLER_MANAGER_PUBLIC + void shutdown_async_compontents(); + protected: CONTROLLER_MANAGER_PUBLIC void init_services(); @@ -515,65 +523,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 6b6efb0a5c..17e60a515e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -500,7 +500,7 @@ 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_)); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1894,6 +1894,12 @@ 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..6ed4a68b64 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -85,6 +85,9 @@ 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..6806caeb8d 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/actuator.hpp" #include "hardware_interface/sensor.hpp" @@ -34,24 +35,15 @@ 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) - { + template + void register_component(T* component) { + hardware_component_ = component; } AsyncComponentThread(const AsyncComponentThread & t) = delete; @@ -88,8 +80,8 @@ class AsyncComponentThread auto measured_period = current_time - previous_time; previous_time = current_time; - // write - // read + component->write(clock_interface_->get_clock()->now(), measured_period); + component->read(clock_interface_->get_clock()->now(), measured_period); } next_iteration_time += period; std::this_thread::sleep_until(next_iteration_time); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a5113380e1..1f74c944cc 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -365,6 +365,9 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hadware interfaces are implemented adequately. */ + + void shutdown_async_components(); + HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); /// Write all loaded hardware components. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e23c34d94..6d3c353d3c 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; @@ -1190,6 +1192,12 @@ 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) @@ -1372,6 +1380,11 @@ void ResourceManager::activate_all_components() { set_component_state(component.get_name(), active_state); } + + for (auto & component : resource_storage_->async_systems_) + { + set_component_state(component.get_name(), active_state); + } } } // namespace hardware_interface From b4ca7904ee0e33d612684c7d995a0e8e13acc765 Mon Sep 17 00:00:00 2001 From: VX792 Date: Mon, 29 May 2023 13:01:04 +0200 Subject: [PATCH 02/11] add header guards + license --- .../controller_interface/async_controller.hpp | 21 ++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 181b0c1ac3..d55b8f83b8 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -1,3 +1,20 @@ +// 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 CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ +#define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ + #include #include #include @@ -75,4 +92,6 @@ class AsyncControllerThread unsigned int cm_update_rate_; }; -} \ No newline at end of file +} + +#endif // CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ From 871537fa4461af4271d514ad175972357a008d7f Mon Sep 17 00:00:00 2001 From: VX792 Date: Mon, 29 May 2023 14:52:52 +0200 Subject: [PATCH 03/11] precommit --- .../controller_interface/async_controller.hpp | 130 ++++++++++-------- .../controller_manager/controller_manager.hpp | 12 +- controller_manager/src/controller_manager.cpp | 8 +- controller_manager/src/ros2_control_node.cpp | 1 - .../hardware_interface/async_components.hpp | 21 ++- .../hardware_interface/resource_manager.hpp | 9 +- hardware_interface/src/resource_manager.cpp | 5 +- 7 files changed, 110 insertions(+), 76 deletions(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index d55b8f83b8..6175d25a2f 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -15,83 +15,97 @@ #ifndef CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ #define CONTROLLER_INTERFACE__ASYNC_CONTROLLER_HPP_ -#include -#include #include +#include +#include -#include "lifecycle_msgs/msg/state.hpp" #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) { - public: - 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) = default; - ~AsyncControllerThread() - { - terminated_.store(true, std::memory_order_seq_cst); - if (thread_.joinable()) - { - thread_.join(); - } - } + AsyncControllerThread(const AsyncControllerThread & t) = delete; + AsyncControllerThread(AsyncControllerThread && t) = delete; - void activate() + // Destructor, called when the component is erased from its map. + ~AsyncControllerThread() + { + terminated_.store(true, std::memory_order_seq_cst); + if (thread_.joinable()) { - thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); + 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(); - void controller_update_callback() + auto previous_time = controller_->get_node()->now(); + while (!terminated_.load(std::memory_order_relaxed)) { - 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 + 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())); - rclcpp::Time previous_time = controller_->get_node()->now(); - - - while (!terminated_.load(std::memory_order_relaxed)) + if (controller_->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - 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); + 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_; - }; +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_ +#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 8cdc2148c3..e09f3f2f3d 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -23,11 +23,10 @@ #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" -#include "controller_interface/async_controller.hpp" - #include "controller_manager/controller_spec.hpp" #include "controller_manager/visibility_control.h" @@ -192,12 +191,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(); - CONTROLLER_MANAGER_PUBLIC - void shutdown_async_compontents(); - protected: CONTROLLER_MANAGER_PUBLIC void init_services(); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 17e60a515e..95594077fd 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -500,7 +500,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_)); } // CHAINABLE CONTROLLERS: get reference interfaces from chainable controllers @@ -1894,9 +1895,10 @@ std::pair ControllerManager::split_command_interface( unsigned int ControllerManager::get_update_rate() const { return update_rate_; } -void ControllerManager::shutdown_async_controllers_and_components() +void ControllerManager::shutdown_async_controllers_and_components() { - async_controller_threads_.erase(async_controller_threads_.begin(), async_controller_threads_.end()); + async_controller_threads_.erase( + async_controller_threads_.begin(), async_controller_threads_.end()); resource_manager_->shutdown_async_components(); } diff --git a/controller_manager/src/ros2_control_node.cpp b/controller_manager/src/ros2_control_node.cpp index 6ed4a68b64..9f8db4f3d9 100644 --- a/controller_manager/src/ros2_control_node.cpp +++ b/controller_manager/src/ros2_control_node.cpp @@ -87,7 +87,6 @@ int main(int argc, char ** argv) } 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 6806caeb8d..ceb8f41087 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -17,8 +17,8 @@ #include #include -#include #include +#include #include "hardware_interface/actuator.hpp" #include "hardware_interface/sensor.hpp" @@ -41,14 +41,17 @@ class AsyncComponentThread { } + // Fills the internal variant with the desired component. template - void register_component(T* component) { + 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); @@ -57,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; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 1f74c944cc..a588ce691d 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -358,6 +358,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. @@ -365,9 +371,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * Part of the real-time critical update loop. * It is realtime-safe if used hadware interfaces are implemented adequately. */ - - void shutdown_async_components(); - HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period); /// Write all loaded hardware components. diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6d3c353d3c..696b2b7c4b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1194,8 +1194,9 @@ return_type ResourceManager::set_component_state( void ResourceManager::shutdown_async_components() { - resource_storage_->async_component_threads_.erase(resource_storage_->async_component_threads_.begin(), - resource_storage_->async_component_threads_.end()); + resource_storage_->async_component_threads_.erase( + resource_storage_->async_component_threads_.begin(), + resource_storage_->async_component_threads_.end()); } // CM API: Called in "update"-thread From c5f02a179a10695e947ace73ff6114cb18cc9de1 Mon Sep 17 00:00:00 2001 From: VX792 Date: Mon, 29 May 2023 15:15:39 +0200 Subject: [PATCH 04/11] remove stuff added to a deprecated fn --- hardware_interface/src/resource_manager.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 696b2b7c4b..fd4a172268 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1381,11 +1381,6 @@ void ResourceManager::activate_all_components() { set_component_state(component.get_name(), active_state); } - - for (auto & component : resource_storage_->async_systems_) - { - set_component_state(component.get_name(), active_state); - } } } // namespace hardware_interface From 7f6eb2991993a8b83ab9581bd3baa5043561eb75 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Wed, 25 Oct 2023 18:10:51 +0100 Subject: [PATCH 05/11] reformat --- .../controller_interface/async_controller.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 6175d25a2f..5968bcf42d 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -30,10 +30,10 @@ 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. - */ + * + * \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) @@ -55,9 +55,9 @@ class AsyncControllerThread /// Creates the controller's thread. /** - * Called when the controller is activated. - * - */ + * Called when the controller is activated. + * + */ void activate() { thread_ = std::thread(&AsyncControllerThread::controller_update_callback, this); From ed305de86ece48b85abc3d054ebbe08bf5d9dbab Mon Sep 17 00:00:00 2001 From: VX792 Date: Sat, 11 Nov 2023 23:08:22 +0100 Subject: [PATCH 06/11] Rework handles to support atomic doubles --- .../include/hardware_interface/handle.hpp | 100 ++++++++++++++---- .../loaned_command_interface.hpp | 28 +++-- .../loaned_state_interface.hpp | 27 +++-- hardware_interface/test/test_handle.cpp | 37 +++++++ .../include/transmission_interface/handle.hpp | 8 +- 5 files changed, 158 insertions(+), 42 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 1aea017754..b493ba5814 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,6 +17,8 @@ #include #include +#include +#include #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -24,28 +26,31 @@ namespace hardware_interface { /// A handle used to get and set a value on a given interface. + + +template class ReadOnlyHandle { + static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadOnlyHandle. Only floating point, and atomic double types are supported for now."); public: ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) + T* value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), value_ptr_((T*)nullptr) { } explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_(nullptr) + : interface_name_(interface_name), value_ptr_((T*)nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; - + ReadOnlyHandle(const ReadOnlyHandle & other) = default; ReadOnlyHandle(ReadOnlyHandle && other) = default; ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; @@ -70,60 +75,82 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } - double get_value() const - { + template + typename std::enable_if_t::value, U> get_value() const + { THROW_ON_NULLPTR(value_ptr_); return *value_ptr_; } + template + typename std::enable_if_t>::value, double> get_value() const + { + THROW_ON_NULLPTR(value_ptr_); + return value_ptr_->load(std::memory_order_relaxed); + } + protected: std::string prefix_name_; std::string interface_name_; - double * value_ptr_; + + T* value_ptr_; + }; -class ReadWriteHandle : public ReadOnlyHandle +template +class ReadWriteHandle : public ReadOnlyHandle { + static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadWriteHandle. Only floating point, and atomic double types are supported for now."); public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, - double * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + T * value_ptr = nullptr) + : ReadOnlyHandle(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - ReadWriteHandle(const ReadWriteHandle & other) = default; + ReadWriteHandle(const ReadWriteHandle & other) : ReadOnlyHandle(other) {} - ReadWriteHandle(ReadWriteHandle && other) = default; + ReadWriteHandle(ReadWriteHandle && other) : ReadOnlyHandle(other) {} - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; ReadWriteHandle & operator=(ReadWriteHandle && other) = default; virtual ~ReadWriteHandle() = default; - void set_value(double value) + template + std::enable_if_t::value, void> set_value(T value) + { + //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle::value_ptr_)); + //std::get<1>(ReadOnlyHandle::value_ptr_)->store(value, std::memory_order_relaxed); + THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); + *(ReadOnlyHandle::value_ptr_) = value; + } + + template + std::enable_if_t>::value, void> set_value(T value) { - THROW_ON_NULLPTR(this->value_ptr_); - *this->value_ptr_ = value; + THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); + ReadOnlyHandle::value_ptr_->store(value, std::memory_order_relaxed); } }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public ReadOnlyHandle { public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using ReadOnlyHandle::ReadOnlyHandle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public ReadWriteHandle { public: /// CommandInterface copy constructor is actively deleted. @@ -136,9 +163,36 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; + using ReadWriteHandle::ReadWriteHandle; +}; + +class AsyncStateInterface : public ReadOnlyHandle> +{ +public: + AsyncStateInterface(const AsyncStateInterface & other) = default; + + AsyncStateInterface(AsyncStateInterface && other) = default; + + using ReadOnlyHandle>::ReadOnlyHandle; +}; + +class AsyncCommandInterface : public ReadWriteHandle> +{ +public: + /// CommandInterface copy constructor is actively deleted. + /** + * Command interfaces are having a unique ownership and thus + * can't be copied in order to avoid simultaneous writes to + * the same resource. + */ + AsyncCommandInterface(const AsyncCommandInterface & other) = delete; + + AsyncCommandInterface(AsyncCommandInterface && other) = default; + + using ReadWriteHandle>::ReadWriteHandle; }; + } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index bb5807c398..29c3d9b9cd 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" @@ -34,7 +35,17 @@ class LoanedCommandInterface } LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) - : command_interface_(command_interface), deleter_(std::forward(deleter)) + : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(false) + { + } + + explicit LoanedCommandInterface(AsyncCommandInterface & command_interface) + : LoanedCommandInterface(command_interface, nullptr) + { + } + + LoanedCommandInterface(AsyncCommandInterface & command_interface, Deleter && deleter) + : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(true) { } @@ -50,26 +61,27 @@ class LoanedCommandInterface } } - const std::string get_name() const { return command_interface_.get_name(); } + const std::string get_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); } - const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_interface_name() : std::get<0>(command_interface_)->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return command_interface_.get_name(); + return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); } - const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_prefix_name() : std::get<0>(command_interface_)->get_prefix_name(); } - void set_value(double val) { command_interface_.set_value(val); } + void set_value(double val) { using_async_command_interface_ ? std::get<1>(command_interface_)->set_value(val) : std::get<0>(command_interface_)->set_value(val); } - double get_value() const { return command_interface_.get_value(); } + double get_value() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_value() : std::get<0>(command_interface_)->get_value(); } protected: - CommandInterface & command_interface_; + std::variant command_interface_; Deleter deleter_; + const bool using_async_command_interface_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index 4fe67d1290..c3e0cd4bde 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" @@ -34,7 +35,17 @@ class LoanedStateInterface } LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) - : state_interface_(state_interface), deleter_(std::forward(deleter)) + : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(false) + { + } + + explicit LoanedStateInterface(AsyncStateInterface & state_interface) + : LoanedStateInterface(state_interface, nullptr) + { + } + + LoanedStateInterface(AsyncStateInterface & state_interface, Deleter && deleter) + : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(true) { } @@ -50,24 +61,26 @@ class LoanedStateInterface } } - const std::string get_name() const { return state_interface_.get_name(); } + const std::string get_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); } - const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } + const std::string & get_interface_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_interface_name() : std::get<0>(state_interface_)->get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return state_interface_.get_name(); + return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); } - const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } + const std::string & get_prefix_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_prefix_name() : std::get<0>(state_interface_)->get_prefix_name(); } - double get_value() const { return state_interface_.get_value(); } + double get_value() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_value() : std::get<0>(state_interface_)->get_value(); } protected: - StateInterface & state_interface_; + std::variant state_interface_; Deleter deleter_; + const bool using_async_state_interface_ = false; + }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 16ca710e9d..3975616e0f 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -12,16 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include "hardware_interface/handle.hpp" using hardware_interface::CommandInterface; using hardware_interface::StateInterface; +using hardware_interface::AsyncCommandInterface; +using hardware_interface::AsyncStateInterface; + + namespace { constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; +constexpr auto BAR_INTERFACE = "BarInterface"; + } // namespace TEST(TestHandle, command_interface) @@ -68,3 +75,33 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } + +TEST(TestHandle, no_data_race_when_accessing_value) // fails when used with regular handles due to thread sanitizer +{ + std::atomic state_if_value = 1.558; + std::atomic cmd_if_value = 1.337; + + AsyncStateInterface state_handle{JOINT_NAME, FOO_INTERFACE, &state_if_value}; + AsyncCommandInterface command_handle{JOINT_NAME, FOO_INTERFACE, &cmd_if_value}; + + + std::thread hwif_read = std::thread([&]() { + state_if_value.store(1.338, std::memory_order_relaxed); + }); + + std::thread controller_update = std::thread([&]() { + command_handle.set_value(state_handle.get_value() + 0.33); + }); + + std::thread hwif_write = std::thread([&]() { + double k = command_handle.get_value(); + cmd_if_value.store(2.0, std::memory_order_relaxed); + }); + + + hwif_read.join(); + controller_update.join(); + hwif_write.join(); + + EXPECT_TRUE(true); +} \ No newline at end of file diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index bc1c0a78d8..adfc3b508d 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; } // namespace transmission_interface From ad1510a66cbb5caef22a9a2081cfc882a48a0633 Mon Sep 17 00:00:00 2001 From: VX792 Date: Sat, 11 Nov 2023 23:23:35 +0100 Subject: [PATCH 07/11] remove comment --- hardware_interface/include/hardware_interface/handle.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index b493ba5814..9f68a825bf 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -126,8 +126,6 @@ class ReadWriteHandle : public ReadOnlyHandle template std::enable_if_t::value, void> set_value(T value) { - //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle::value_ptr_)); - //std::get<1>(ReadOnlyHandle::value_ptr_)->store(value, std::memory_order_relaxed); THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); *(ReadOnlyHandle::value_ptr_) = value; } From 58f90b2f75978f8ba848703a28aea4bbacde776c Mon Sep 17 00:00:00 2001 From: VX792 Date: Thu, 4 Apr 2024 14:44:32 +0200 Subject: [PATCH 08/11] Revert "remove comment" This reverts commit ad1510a66cbb5caef22a9a2081cfc882a48a0633. --- hardware_interface/include/hardware_interface/handle.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index 9f68a825bf..b493ba5814 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -126,6 +126,8 @@ class ReadWriteHandle : public ReadOnlyHandle template std::enable_if_t::value, void> set_value(T value) { + //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle::value_ptr_)); + //std::get<1>(ReadOnlyHandle::value_ptr_)->store(value, std::memory_order_relaxed); THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); *(ReadOnlyHandle::value_ptr_) = value; } From 8befba391b26a5fdf18d1ecf529951472e7864dc Mon Sep 17 00:00:00 2001 From: VX792 Date: Thu, 4 Apr 2024 14:44:44 +0200 Subject: [PATCH 09/11] Revert "Rework handles to support atomic doubles" This reverts commit ed305de86ece48b85abc3d054ebbe08bf5d9dbab. --- .../include/hardware_interface/handle.hpp | 100 ++++-------------- .../loaned_command_interface.hpp | 28 ++--- .../loaned_state_interface.hpp | 27 ++--- hardware_interface/test/test_handle.cpp | 37 ------- .../include/transmission_interface/handle.hpp | 8 +- 5 files changed, 42 insertions(+), 158 deletions(-) diff --git a/hardware_interface/include/hardware_interface/handle.hpp b/hardware_interface/include/hardware_interface/handle.hpp index b493ba5814..1aea017754 100644 --- a/hardware_interface/include/hardware_interface/handle.hpp +++ b/hardware_interface/include/hardware_interface/handle.hpp @@ -17,8 +17,6 @@ #include #include -#include -#include #include "hardware_interface/macros.hpp" #include "hardware_interface/visibility_control.h" @@ -26,31 +24,28 @@ namespace hardware_interface { /// A handle used to get and set a value on a given interface. - - -template class ReadOnlyHandle { - static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadOnlyHandle. Only floating point, and atomic double types are supported for now."); public: ReadOnlyHandle( const std::string & prefix_name, const std::string & interface_name, - T* value_ptr = nullptr) + double * value_ptr = nullptr) : prefix_name_(prefix_name), interface_name_(interface_name), value_ptr_(value_ptr) { } explicit ReadOnlyHandle(const std::string & interface_name) - : interface_name_(interface_name), value_ptr_((T*)nullptr) + : interface_name_(interface_name), value_ptr_(nullptr) { } explicit ReadOnlyHandle(const char * interface_name) - : interface_name_(interface_name), value_ptr_((T*)nullptr) + : interface_name_(interface_name), value_ptr_(nullptr) { } - ReadOnlyHandle(const ReadOnlyHandle & other) = default; + ReadOnlyHandle(const ReadOnlyHandle & other) = default; + ReadOnlyHandle(ReadOnlyHandle && other) = default; ReadOnlyHandle & operator=(const ReadOnlyHandle & other) = default; @@ -75,82 +70,60 @@ class ReadOnlyHandle const std::string & get_prefix_name() const { return prefix_name_; } - template - typename std::enable_if_t::value, U> get_value() const - { - THROW_ON_NULLPTR(value_ptr_); - return *value_ptr_; - } - - template - typename std::enable_if_t>::value, double> get_value() const + double get_value() const { THROW_ON_NULLPTR(value_ptr_); - return value_ptr_->load(std::memory_order_relaxed); + return *value_ptr_; } protected: std::string prefix_name_; std::string interface_name_; - - T* value_ptr_; - + double * value_ptr_; }; -template -class ReadWriteHandle : public ReadOnlyHandle +class ReadWriteHandle : public ReadOnlyHandle { - static_assert(std::is_floating_point::value || std::is_same>::value, "Invalid template argument for class ReadWriteHandle. Only floating point, and atomic double types are supported for now."); public: ReadWriteHandle( const std::string & prefix_name, const std::string & interface_name, - T * value_ptr = nullptr) - : ReadOnlyHandle(prefix_name, interface_name, value_ptr) + double * value_ptr = nullptr) + : ReadOnlyHandle(prefix_name, interface_name, value_ptr) { } - explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const std::string & interface_name) : ReadOnlyHandle(interface_name) {} - explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} + explicit ReadWriteHandle(const char * interface_name) : ReadOnlyHandle(interface_name) {} - ReadWriteHandle(const ReadWriteHandle & other) : ReadOnlyHandle(other) {} + ReadWriteHandle(const ReadWriteHandle & other) = default; - ReadWriteHandle(ReadWriteHandle && other) : ReadOnlyHandle(other) {} + ReadWriteHandle(ReadWriteHandle && other) = default; - ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; + ReadWriteHandle & operator=(const ReadWriteHandle & other) = default; ReadWriteHandle & operator=(ReadWriteHandle && other) = default; virtual ~ReadWriteHandle() = default; - template - std::enable_if_t::value, void> set_value(T value) - { - //THROW_ON_NULLPTR(std::get<1>(ReadOnlyHandle::value_ptr_)); - //std::get<1>(ReadOnlyHandle::value_ptr_)->store(value, std::memory_order_relaxed); - THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); - *(ReadOnlyHandle::value_ptr_) = value; - } - - template - std::enable_if_t>::value, void> set_value(T value) + void set_value(double value) { - THROW_ON_NULLPTR(ReadOnlyHandle::value_ptr_); - ReadOnlyHandle::value_ptr_->store(value, std::memory_order_relaxed); + THROW_ON_NULLPTR(this->value_ptr_); + *this->value_ptr_ = value; } }; -class StateInterface : public ReadOnlyHandle +class StateInterface : public ReadOnlyHandle { public: StateInterface(const StateInterface & other) = default; StateInterface(StateInterface && other) = default; - using ReadOnlyHandle::ReadOnlyHandle; + using ReadOnlyHandle::ReadOnlyHandle; }; -class CommandInterface : public ReadWriteHandle +class CommandInterface : public ReadWriteHandle { public: /// CommandInterface copy constructor is actively deleted. @@ -163,36 +136,9 @@ class CommandInterface : public ReadWriteHandle CommandInterface(CommandInterface && other) = default; - using ReadWriteHandle::ReadWriteHandle; -}; - -class AsyncStateInterface : public ReadOnlyHandle> -{ -public: - AsyncStateInterface(const AsyncStateInterface & other) = default; - - AsyncStateInterface(AsyncStateInterface && other) = default; - - using ReadOnlyHandle>::ReadOnlyHandle; -}; - -class AsyncCommandInterface : public ReadWriteHandle> -{ -public: - /// CommandInterface copy constructor is actively deleted. - /** - * Command interfaces are having a unique ownership and thus - * can't be copied in order to avoid simultaneous writes to - * the same resource. - */ - AsyncCommandInterface(const AsyncCommandInterface & other) = delete; - - AsyncCommandInterface(AsyncCommandInterface && other) = default; - - using ReadWriteHandle>::ReadWriteHandle; + using ReadWriteHandle::ReadWriteHandle; }; - } // namespace hardware_interface #endif // HARDWARE_INTERFACE__HANDLE_HPP_ diff --git a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp index 29c3d9b9cd..bb5807c398 100644 --- a/hardware_interface/include/hardware_interface/loaned_command_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_command_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include "hardware_interface/handle.hpp" @@ -35,17 +34,7 @@ class LoanedCommandInterface } LoanedCommandInterface(CommandInterface & command_interface, Deleter && deleter) - : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(false) - { - } - - explicit LoanedCommandInterface(AsyncCommandInterface & command_interface) - : LoanedCommandInterface(command_interface, nullptr) - { - } - - LoanedCommandInterface(AsyncCommandInterface & command_interface, Deleter && deleter) - : command_interface_(&command_interface), deleter_(std::forward(deleter)), using_async_command_interface_(true) + : command_interface_(command_interface), deleter_(std::forward(deleter)) { } @@ -61,27 +50,26 @@ class LoanedCommandInterface } } - const std::string get_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); } + const std::string get_name() const { return command_interface_.get_name(); } - const std::string & get_interface_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_interface_name() : std::get<0>(command_interface_)->get_interface_name(); } + const std::string & get_interface_name() const { return command_interface_.get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return using_async_command_interface_ ? std::get<1>(command_interface_)->get_name() : std::get<0>(command_interface_)->get_name(); + return command_interface_.get_name(); } - const std::string & get_prefix_name() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_prefix_name() : std::get<0>(command_interface_)->get_prefix_name(); } + const std::string & get_prefix_name() const { return command_interface_.get_prefix_name(); } - void set_value(double val) { using_async_command_interface_ ? std::get<1>(command_interface_)->set_value(val) : std::get<0>(command_interface_)->set_value(val); } + void set_value(double val) { command_interface_.set_value(val); } - double get_value() const { return using_async_command_interface_ ? std::get<1>(command_interface_)->get_value() : std::get<0>(command_interface_)->get_value(); } + double get_value() const { return command_interface_.get_value(); } protected: - std::variant command_interface_; + CommandInterface & command_interface_; Deleter deleter_; - const bool using_async_command_interface_; }; } // namespace hardware_interface diff --git a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp index c3e0cd4bde..4fe67d1290 100644 --- a/hardware_interface/include/hardware_interface/loaned_state_interface.hpp +++ b/hardware_interface/include/hardware_interface/loaned_state_interface.hpp @@ -18,7 +18,6 @@ #include #include #include -#include #include "hardware_interface/handle.hpp" @@ -35,17 +34,7 @@ class LoanedStateInterface } LoanedStateInterface(StateInterface & state_interface, Deleter && deleter) - : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(false) - { - } - - explicit LoanedStateInterface(AsyncStateInterface & state_interface) - : LoanedStateInterface(state_interface, nullptr) - { - } - - LoanedStateInterface(AsyncStateInterface & state_interface, Deleter && deleter) - : state_interface_(&state_interface), deleter_(std::forward(deleter)), using_async_state_interface_(true) + : state_interface_(state_interface), deleter_(std::forward(deleter)) { } @@ -61,26 +50,24 @@ class LoanedStateInterface } } - const std::string get_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); } + const std::string get_name() const { return state_interface_.get_name(); } - const std::string & get_interface_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_interface_name() : std::get<0>(state_interface_)->get_interface_name(); } + const std::string & get_interface_name() const { return state_interface_.get_interface_name(); } [[deprecated( "Replaced by get_name method, which is semantically more correct")]] const std::string get_full_name() const { - return using_async_state_interface_ ? std::get<1>(state_interface_)->get_name() : std::get<0>(state_interface_)->get_name(); + return state_interface_.get_name(); } - const std::string & get_prefix_name() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_prefix_name() : std::get<0>(state_interface_)->get_prefix_name(); } + const std::string & get_prefix_name() const { return state_interface_.get_prefix_name(); } - double get_value() const { return using_async_state_interface_ ? std::get<1>(state_interface_)->get_value() : std::get<0>(state_interface_)->get_value(); } + double get_value() const { return state_interface_.get_value(); } protected: - std::variant state_interface_; + StateInterface & state_interface_; Deleter deleter_; - const bool using_async_state_interface_ = false; - }; } // namespace hardware_interface diff --git a/hardware_interface/test/test_handle.cpp b/hardware_interface/test/test_handle.cpp index 3975616e0f..16ca710e9d 100644 --- a/hardware_interface/test/test_handle.cpp +++ b/hardware_interface/test/test_handle.cpp @@ -12,23 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include "hardware_interface/handle.hpp" using hardware_interface::CommandInterface; using hardware_interface::StateInterface; -using hardware_interface::AsyncCommandInterface; -using hardware_interface::AsyncStateInterface; - - namespace { constexpr auto JOINT_NAME = "joint_1"; constexpr auto FOO_INTERFACE = "FooInterface"; -constexpr auto BAR_INTERFACE = "BarInterface"; - } // namespace TEST(TestHandle, command_interface) @@ -75,33 +68,3 @@ TEST(TestHandle, value_methods_work_on_non_nullptr) EXPECT_NO_THROW(handle.set_value(0.0)); EXPECT_DOUBLE_EQ(handle.get_value(), 0.0); } - -TEST(TestHandle, no_data_race_when_accessing_value) // fails when used with regular handles due to thread sanitizer -{ - std::atomic state_if_value = 1.558; - std::atomic cmd_if_value = 1.337; - - AsyncStateInterface state_handle{JOINT_NAME, FOO_INTERFACE, &state_if_value}; - AsyncCommandInterface command_handle{JOINT_NAME, FOO_INTERFACE, &cmd_if_value}; - - - std::thread hwif_read = std::thread([&]() { - state_if_value.store(1.338, std::memory_order_relaxed); - }); - - std::thread controller_update = std::thread([&]() { - command_handle.set_value(state_handle.get_value() + 0.33); - }); - - std::thread hwif_write = std::thread([&]() { - double k = command_handle.get_value(); - cmd_if_value.store(2.0, std::memory_order_relaxed); - }); - - - hwif_read.join(); - controller_update.join(); - hwif_write.join(); - - EXPECT_TRUE(true); -} \ No newline at end of file diff --git a/transmission_interface/include/transmission_interface/handle.hpp b/transmission_interface/include/transmission_interface/handle.hpp index adfc3b508d..bc1c0a78d8 100644 --- a/transmission_interface/include/transmission_interface/handle.hpp +++ b/transmission_interface/include/transmission_interface/handle.hpp @@ -22,17 +22,17 @@ namespace transmission_interface { /** A handle used to get and set a value on a given actuator interface. */ -class ActuatorHandle : public hardware_interface::ReadWriteHandle +class ActuatorHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; /** A handle used to get and set a value on a given joint interface. */ -class JointHandle : public hardware_interface::ReadWriteHandle +class JointHandle : public hardware_interface::ReadWriteHandle { public: - using hardware_interface::ReadWriteHandle::ReadWriteHandle; + using hardware_interface::ReadWriteHandle::ReadWriteHandle; }; } // namespace transmission_interface From 67da21ce92c395e7d1358937553a0e2e146da6cd Mon Sep 17 00:00:00 2001 From: VX792 Date: Thu, 4 Apr 2024 17:05:27 +0200 Subject: [PATCH 10/11] don't write in the first tick --- .../include/hardware_interface/async_components.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp index ceb8f41087..73495c92f8 100644 --- a/hardware_interface/include/hardware_interface/async_components.hpp +++ b/hardware_interface/include/hardware_interface/async_components.hpp @@ -93,8 +93,12 @@ class AsyncComponentThread auto measured_period = current_time - previous_time; previous_time = current_time; - component->write(clock_interface_->get_clock()->now(), measured_period); + 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); @@ -109,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_; }; From 9469f8b763352392d784feac548275de63402042 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 24 Apr 2024 18:28:36 +0200 Subject: [PATCH 11/11] Update the year in license. --- .../include/controller_interface/async_controller.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_controller.hpp b/controller_interface/include/controller_interface/async_controller.hpp index 5968bcf42d..5601ff4703 100644 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ b/controller_interface/include/controller_interface/async_controller.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 ros2_control development team +// 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.