From e8130a4dce2e436408a7856ab38a6e4478c6f769 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 9 Apr 2024 23:32:50 +0200 Subject: [PATCH 01/84] Add first version of the async controllers --- .../controller_interface_base.hpp | 21 +++++++++- .../src/controller_interface_base.cpp | 40 +++++++++++++++++++ 2 files changed, 60 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 74077969d3..d65a9378c0 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -15,6 +15,7 @@ #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ +#include #include #include #include @@ -70,7 +71,15 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy ControllerInterfaceBase() = default; CONTROLLER_INTERFACE_PUBLIC - virtual ~ControllerInterfaceBase() = default; + virtual ~ControllerInterfaceBase() + { + async_update_stop_ = true; + if (thread_.joinable()) + { + async_update_condition_.notify_one(); + thread_.join(); + } + } /// Get configuration for controller's required command interfaces. /** @@ -153,6 +162,9 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + CONTROLLER_INTERFACE_PUBLIC + return_type trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); + CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); @@ -274,6 +286,13 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; + + // Async related variables + std::thread thread_; + std::atomic async_update_stop_{false}; + std::atomic async_update_return_{return_type::OK}; + std::condition_variable async_update_condition_; + std::mutex async_mtx_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 61d2c780b8..300d4064f9 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -111,6 +111,46 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } +return_type ControllerInterfaceBase::trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + if (is_async()) + { + /// Start a new thread and call the update function using the conditional variable and this + /// conditional variable will be notified when the update function is done and it will wait for + /// the next update which will be when the trigger_update function is called again, and at the + /// same time it should maintain the update rate. This way we can have async callback + // check if the thread_ has a callback running + if (!thread_.joinable()) + { + auto update_fn = [this, time, period]() -> void + { + std::unique_lock lock(async_mtx_); + while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + !async_update_stop_) + { + async_update_condition_.wait(lock); + lock.unlock(); + async_update_return_ = update(time, period); + async_update_condition_.notify_one(); + } + }; + thread_ = std::thread(update_fn); + } + else + { + std::lock_guard lk(async_mtx_); + async_update_condition_.notify_one(); + } + return async_update_return_; + } + else + { + return update(time, period); + } + return return_type::OK; +} + std::shared_ptr ControllerInterfaceBase::get_node() { if (!node_.get()) From 062a4452c0a1e3d9edaa907d6f3745760d9d1ea3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 15:02:31 +0200 Subject: [PATCH 02/84] removed extra notify one --- controller_interface/src/controller_interface_base.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 300d4064f9..f1d9ee87ab 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -132,7 +132,6 @@ return_type ControllerInterfaceBase::trigger_update( async_update_condition_.wait(lock); lock.unlock(); async_update_return_ = update(time, period); - async_update_condition_.notify_one(); } }; thread_ = std::thread(update_fn); From fd270ca77e9360f03d77d0c8c3a58b5f3b4e3563 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:11:47 +0200 Subject: [PATCH 03/84] added async_update_ready_ to use with conditional variable to avoid spurious wakeups --- .../controller_interface_base.hpp | 1 + .../src/controller_interface_base.cpp | 13 +++++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index d65a9378c0..e8cf9cc336 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -290,6 +290,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy // Async related variables std::thread thread_; std::atomic async_update_stop_{false}; + std::atomic async_update_ready_{false}; std::atomic async_update_return_{return_type::OK}; std::condition_variable async_update_condition_; std::mutex async_mtx_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index f1d9ee87ab..9a4033860a 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -129,17 +129,22 @@ return_type ControllerInterfaceBase::trigger_update( while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { - async_update_condition_.wait(lock); - lock.unlock(); + async_update_condition_.wait(lock, []{ return async_update_ready_;}); async_update_return_ = update(time, period); + async_update_ready_ = false; + lock.unlock(); } }; thread_ = std::thread(update_fn); } else { - std::lock_guard lk(async_mtx_); - async_update_condition_.notify_one(); + if(!async_update_ready_) + { + std::lock_guard lk(async_mtx_); + async_update_condition_.notify_one(); + async_update_ready_ = true; + } } return async_update_return_; } From 703f91fc203a6427ea250944c39318a05c2bb044 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:23:05 +0200 Subject: [PATCH 04/84] call notify_one after setting the async_update_ready --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 9a4033860a..445f8c106d 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -142,8 +142,8 @@ return_type ControllerInterfaceBase::trigger_update( if(!async_update_ready_) { std::lock_guard lk(async_mtx_); - async_update_condition_.notify_one(); async_update_ready_ = true; + async_update_condition_.notify_one(); } } return async_update_return_; From c09339f8245cfafda2976098efb02d7b4e8cb656 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:24:33 +0200 Subject: [PATCH 05/84] add precommit changes --- controller_interface/src/controller_interface_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 445f8c106d..95583f5020 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -129,7 +129,7 @@ return_type ControllerInterfaceBase::trigger_update( while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { - async_update_condition_.wait(lock, []{ return async_update_ready_;}); + async_update_condition_.wait(lock, [] { return async_update_ready_; }); async_update_return_ = update(time, period); async_update_ready_ = false; lock.unlock(); @@ -139,7 +139,7 @@ return_type ControllerInterfaceBase::trigger_update( } else { - if(!async_update_ready_) + if (!async_update_ready_) { std::lock_guard lk(async_mtx_); async_update_ready_ = true; From 1f58a2d972344a65f9b7054ad89f63742cdefeef Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:34:11 +0200 Subject: [PATCH 06/84] add missing this in the conditional variable callback --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 95583f5020..fe6d76f771 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -129,7 +129,7 @@ return_type ControllerInterfaceBase::trigger_update( while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { - async_update_condition_.wait(lock, [] { return async_update_ready_; }); + async_update_condition_.wait(lock, [this] { return async_update_ready_; }); async_update_return_ = update(time, period); async_update_ready_ = false; lock.unlock(); From c2f7192c22835ed426d28c7b2557546431e81945 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:34:46 +0200 Subject: [PATCH 07/84] change atomic bool to normal bool --- .../include/controller_interface/controller_interface_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index e8cf9cc336..c879ddf105 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -290,7 +290,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy // Async related variables std::thread thread_; std::atomic async_update_stop_{false}; - std::atomic async_update_ready_{false}; + bool async_update_ready_{false}; std::atomic async_update_return_{return_type::OK}; std::condition_variable async_update_condition_; std::mutex async_mtx_; From 4255cc39aa27de54f6e9a2f4b55bc1868c794e52 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Apr 2024 18:35:10 +0200 Subject: [PATCH 08/84] add wait_for_update_to_finish method --- .../controller_interface_base.hpp | 11 +++++++++++ .../src/controller_interface_base.cpp | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index c879ddf105..5dfe7d346f 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -280,6 +280,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /// A method to wait until the controller update method is done. + CONTROLLER_INTERFACE_PUBLIC + void wait_for_update_to_finish() + { + if (is_async()) + { + std::unique_lock lock(async_mtx_); + async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); + } + } + protected: std::vector command_interfaces_; std::vector state_interfaces_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index fe6d76f771..64d738110f 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -143,7 +143,7 @@ return_type ControllerInterfaceBase::trigger_update( { std::lock_guard lk(async_mtx_); async_update_ready_ = true; - async_update_condition_.notify_one(); + async_update_condition_.notify_all(); } } return async_update_return_; From 9f7c0b4fb55e92ba3d71d01b66b0c9799abb97d3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 09:55:50 +0200 Subject: [PATCH 09/84] add notify_one in different places to wait properly to finish update cycle --- controller_interface/src/controller_interface_base.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 64d738110f..1c50bb97c8 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -133,6 +133,7 @@ return_type ControllerInterfaceBase::trigger_update( async_update_return_ = update(time, period); async_update_ready_ = false; lock.unlock(); + async_update_condition_.notify_one(); } }; thread_ = std::thread(update_fn); @@ -143,7 +144,7 @@ return_type ControllerInterfaceBase::trigger_update( { std::lock_guard lk(async_mtx_); async_update_ready_ = true; - async_update_condition_.notify_all(); + async_update_condition_.notify_one(); } } return async_update_return_; From faba7cd3cc46431e05b5d80932b6abb76ec5ca07 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 10:07:18 +0200 Subject: [PATCH 10/84] added unique lock and check with try_lock for triggering calls --- controller_interface/src/controller_interface_base.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 1c50bb97c8..9a6ba09943 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -140,12 +140,13 @@ return_type ControllerInterfaceBase::trigger_update( } else { - if (!async_update_ready_) + std::unique_lock lock(async_mtx_); + if (!async_update_ready_ && lock.try_lock()) { - std::lock_guard lk(async_mtx_); async_update_ready_ = true; async_update_condition_.notify_one(); } + lock.unlock(); } return async_update_return_; } From 4ffa8851c4b4a7c0fe671e52bca40b12c10413ac Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 11:14:07 +0200 Subject: [PATCH 11/84] use try_to_lock for better checking of owning the lock or not --- controller_interface/src/controller_interface_base.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 9a6ba09943..6f781c63a9 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -140,13 +140,12 @@ return_type ControllerInterfaceBase::trigger_update( } else { - std::unique_lock lock(async_mtx_); - if (!async_update_ready_ && lock.try_lock()) + std::unique_lock lock(async_mtx_, std::try_to_lock); + if (!async_update_ready_ && lock.owns_lock()) { async_update_ready_ = true; async_update_condition_.notify_one(); } - lock.unlock(); } return async_update_return_; } From f575440c0fcc320cd381d72f1e12fb4a2107f946 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 15:19:29 +0200 Subject: [PATCH 12/84] add current update time and period to be able to update them --- .../controller_interface_base.hpp | 2 ++ .../src/controller_interface_base.cpp | 12 +++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 5dfe7d346f..0aabc081e9 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -297,6 +297,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; + rclcpp::Time current_update_time_; + rclcpp::Duration current_update_period_; // Async related variables std::thread thread_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 6f781c63a9..45eb5e59cc 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -123,14 +123,16 @@ return_type ControllerInterfaceBase::trigger_update( // check if the thread_ has a callback running if (!thread_.joinable()) { - auto update_fn = [this, time, period]() -> void + auto update_fn = [this]() -> void { + current_update_time_ = time; + current_update_period_ = period; std::unique_lock lock(async_mtx_); while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { async_update_condition_.wait(lock, [this] { return async_update_ready_; }); - async_update_return_ = update(time, period); + async_update_return_ = update(current_update_time_, current_update_period_); async_update_ready_ = false; lock.unlock(); async_update_condition_.notify_one(); @@ -144,6 +146,8 @@ return_type ControllerInterfaceBase::trigger_update( if (!async_update_ready_ && lock.owns_lock()) { async_update_ready_ = true; + current_update_time_ = time; + current_update_period_ = period; async_update_condition_.notify_one(); } } @@ -151,7 +155,9 @@ return_type ControllerInterfaceBase::trigger_update( } else { - return update(time, period); + current_update_time_ = time; + current_update_period_ = period; + return update(current_update_time_, current_update_period_); } return return_type::OK; } From 434801907c35cc2717d96402e93bf9b9fff670cd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 16:55:59 +0200 Subject: [PATCH 13/84] update documentation of the async method --- .../src/controller_interface_base.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 45eb5e59cc..119a61c172 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -116,11 +116,12 @@ return_type ControllerInterfaceBase::trigger_update( { if (is_async()) { - /// Start a new thread and call the update function using the conditional variable and this - /// conditional variable will be notified when the update function is done and it will wait for - /// the next update which will be when the trigger_update function is called again, and at the - /// same time it should maintain the update rate. This way we can have async callback - // check if the thread_ has a callback running + // * If the thread is not joinable, create a new thread and call the update function and wait + // for it to be triggered again and repeat the same process. + // * If the thread is joinable, check if the current update cycle is finished. If so, then + // trigger a new update cycle. + // * The controller managr is responsible for triggering the update cycle and maintaining the + // controller's update rate and should be acting as a scheduler. if (!thread_.joinable()) { auto update_fn = [this]() -> void From a2469ca787584ea2856e2ac4245e7a8c8dd8e2e9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 16:59:51 +0200 Subject: [PATCH 14/84] added some comments about the issue with the get_state method --- controller_interface/src/controller_interface_base.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 119a61c172..d8b2985327 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -129,6 +129,9 @@ return_type ControllerInterfaceBase::trigger_update( current_update_time_ = time; current_update_period_ = period; std::unique_lock lock(async_mtx_); + // \note might be an concurrency issue with the get_state() call here. This mightn't be + // critical here as the state of the controller is not expected to change during the update + // cycle while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { From ffaad19936fac8efbd2c8a5228f19332f85b3cc6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 18:38:07 +0200 Subject: [PATCH 15/84] use atomic_bool --- .../include/controller_interface/controller_interface_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 0aabc081e9..63b7745477 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -302,7 +302,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy // Async related variables std::thread thread_; - std::atomic async_update_stop_{false}; + std::atomic_bool async_update_stop_{false}; bool async_update_ready_{false}; std::atomic async_update_return_{return_type::OK}; std::condition_variable async_update_condition_; From b7586a2ffd5a23d13eedc3a224bfd45a10c8f2cf Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 18:38:27 +0200 Subject: [PATCH 16/84] minor change in the logic --- .../src/controller_interface_base.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index d8b2985327..6d9d464de6 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -126,8 +126,8 @@ return_type ControllerInterfaceBase::trigger_update( { auto update_fn = [this]() -> void { - current_update_time_ = time; - current_update_period_ = period; + async_update_stop_ = false; + async_update_ready_ = false; std::unique_lock lock(async_mtx_); // \note might be an concurrency issue with the get_state() call here. This mightn't be // critical here as the state of the controller is not expected to change during the update @@ -144,16 +144,13 @@ return_type ControllerInterfaceBase::trigger_update( }; thread_ = std::thread(update_fn); } - else + std::unique_lock lock(async_mtx_, std::try_to_lock); + if (!async_update_ready_ && lock.owns_lock()) { - std::unique_lock lock(async_mtx_, std::try_to_lock); - if (!async_update_ready_ && lock.owns_lock()) - { - async_update_ready_ = true; - current_update_time_ = time; - current_update_period_ = period; - async_update_condition_.notify_one(); - } + async_update_ready_ = true; + current_update_time_ = time; + current_update_period_ = period; + async_update_condition_.notify_one(); } return async_update_return_; } From 0772c4918d5b63724c1bb439cc071f868b44446e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 18:56:10 +0200 Subject: [PATCH 17/84] initialize the duration to fix the compilation error --- .../include/controller_interface/controller_interface_base.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 63b7745477..94ef6f51e4 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -298,7 +298,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; rclcpp::Time current_update_time_; - rclcpp::Duration current_update_period_; + rclcpp::Duration current_update_period_{0, 0}; // Async related variables std::thread thread_; From bf2a2d3555476ffb1cefecf8196b9fa0f940f87f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 19:18:59 +0200 Subject: [PATCH 18/84] Move the main async logic into a separate method --- .../controller_interface_base.hpp | 2 + .../src/controller_interface_base.cpp | 61 ++++++++++--------- 2 files changed, 35 insertions(+), 28 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 94ef6f51e4..126311087c 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -296,6 +296,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy std::vector state_interfaces_; private: + void initialize_async_update_thread(); + std::shared_ptr node_; rclcpp::Time current_update_time_; rclcpp::Duration current_update_period_{0, 0}; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 6d9d464de6..e8446d1815 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -116,34 +116,7 @@ return_type ControllerInterfaceBase::trigger_update( { if (is_async()) { - // * If the thread is not joinable, create a new thread and call the update function and wait - // for it to be triggered again and repeat the same process. - // * If the thread is joinable, check if the current update cycle is finished. If so, then - // trigger a new update cycle. - // * The controller managr is responsible for triggering the update cycle and maintaining the - // controller's update rate and should be acting as a scheduler. - if (!thread_.joinable()) - { - auto update_fn = [this]() -> void - { - async_update_stop_ = false; - async_update_ready_ = false; - std::unique_lock lock(async_mtx_); - // \note might be an concurrency issue with the get_state() call here. This mightn't be - // critical here as the state of the controller is not expected to change during the update - // cycle - while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && - !async_update_stop_) - { - async_update_condition_.wait(lock, [this] { return async_update_ready_; }); - async_update_return_ = update(current_update_time_, current_update_period_); - async_update_ready_ = false; - lock.unlock(); - async_update_condition_.notify_one(); - } - }; - thread_ = std::thread(update_fn); - } + initialize_async_update_thread(); std::unique_lock lock(async_mtx_, std::try_to_lock); if (!async_update_ready_ && lock.owns_lock()) { @@ -187,4 +160,36 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +void ControllerInterfaceBase::initialize_async_update_thread() +{ + // * If the thread is not joinable, create a new thread and call the update function and wait + // for it to be triggered again and repeat the same process. + // * If the thread is joinable, check if the current update cycle is finished. If so, then + // trigger a new update cycle. + // * The controller managr is responsible for triggering the update cycle and maintaining the + // controller's update rate and should be acting as a scheduler. + if (is_async() && !thread_.joinable()) + { + auto update_fn = [this]() -> void + { + async_update_stop_ = false; + async_update_ready_ = false; + std::unique_lock lock(async_mtx_); + // \note There might be an concurrency issue with the get_state() call here. This mightn't be + // critical here as the state of the controller is not expected to change during the update + // cycle + while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + !async_update_stop_) + { + async_update_condition_.wait(lock, [this] { return async_update_ready_; }); + async_update_return_ = update(current_update_time_, current_update_period_); + async_update_ready_ = false; + lock.unlock(); + async_update_condition_.notify_one(); + } + }; + thread_ = std::thread(update_fn); + } +} + } // namespace controller_interface From 89e6a7b780069d303a95405f8b5fa4c8fffe1b7f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 11 Apr 2024 23:15:13 +0200 Subject: [PATCH 19/84] Add async function handler to handle parsed functions --- .../async_function_handler.hpp | 140 ++++++++++++++++++ 1 file changed, 140 insertions(+) create mode 100644 controller_interface/include/controller_interface/async_function_handler.hpp diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp new file mode 100644 index 0000000000..b1727719c1 --- /dev/null +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -0,0 +1,140 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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_FUNCTION_HANDLER_HPP_ +#define CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" + +namespace ros2_control +{ +template +class AsyncFunctionHandler +{ +public: + AsyncFunctionHandler() = default; + + ~AsyncFunctionHandler() + { + async_update_stop_ = true; + if (thread_.joinable()) + { + async_update_condition_.notify_one(); + thread_.join(); + } + } + + void init( + std::function get_state_function, + std::function async_function) + { + if (thread_.joinable()) + { + throw std::runtime_error("AsyncFunctionHandler already initialized!"); + } + get_state_function_ = get_state_function; + async_function_ = async_function; + } + + T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) + { + initialize_async_update_thread(get_state_function_, async_function_); + std::unique_lock lock(async_mtx_, std::try_to_lock); + if (!async_update_ready_ && lock.owns_lock()) + { + async_update_ready_ = true; + current_update_time_ = time; + current_update_period_ = period; + async_update_condition_.notify_one(); + } + return async_update_return_; + } + + void wait_for_update_to_finish() + { + std::unique_lock lock(async_mtx_); + async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); + } + + bool is_initialized() const + { + return thread_.joinable() && get_state_function_ != nullptr && async_function_ != nullptr; + } + +private: + void initialize_async_update_thread( + std::function get_state_function, + std::function async_function) + { + // * If the thread is not joinable, create a new thread and call the update function and wait + // for it to be triggered again and repeat the same process. + // * If the thread is joinable, check if the current update cycle is finished. If so, then + // trigger a new update cycle. + // * The controller managr is responsible for triggering the update cycle and maintaining the + // controller's update rate and should be acting as a scheduler. + if (get_state_function_ == nullptr || async_function_ == nullptr) + { + throw std::runtime_error("AsyncFunctionHandler not initialized properly!"); + } + if (!thread_.joinable()) + { + auto update_fn = [this, get_state_function, async_function]() -> void + { + async_update_stop_ = false; + async_update_ready_ = false; + std::unique_lock lock(async_mtx_); + // \note There might be an concurrency issue with the get_state() call here. This mightn't + // be critical here as the state of the controller is not expected to change during the + // update cycle + while (get_state_function().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + !async_update_stop_) + { + async_update_condition_.wait(lock, [this] { return async_update_ready_; }); + async_update_return_ = async_function(current_update_time_, current_update_period_); + async_update_ready_ = false; + lock.unlock(); + async_update_condition_.notify_one(); + } + }; + thread_ = std::thread(update_fn); + } + } + + rclcpp::Time current_update_time_; + rclcpp::Duration current_update_period_{0, 0}; + + std::function get_state_function_; + std::function async_function_; + + // Async related variables + std::thread thread_; + std::atomic_bool async_update_stop_{false}; + bool async_update_ready_{false}; + std::atomic async_update_return_; + std::condition_variable async_update_condition_; + std::mutex async_mtx_; +}; +} // namespace ros2_control + +#endif // CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ From 2e0a0d2b103c4a337822308a1487cfd044867abd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 08:55:44 +0200 Subject: [PATCH 20/84] add missing template typenames on functions --- .../include/controller_interface/async_function_handler.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index b1727719c1..67ddc7df14 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -45,6 +45,7 @@ class AsyncFunctionHandler } } + template void init( std::function get_state_function, std::function async_function) @@ -57,6 +58,7 @@ class AsyncFunctionHandler async_function_ = async_function; } + template T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) { initialize_async_update_thread(get_state_function_, async_function_); @@ -83,6 +85,7 @@ class AsyncFunctionHandler } private: + template void initialize_async_update_thread( std::function get_state_function, std::function async_function) From bfae6a1d6bfa903194e57d7b9dc10bbf936b85da Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 11:02:48 +0200 Subject: [PATCH 21/84] set wait_for_update_to_finish to const --- .../include/controller_interface/async_function_handler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 67ddc7df14..c1d2a523e3 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -73,7 +73,7 @@ class AsyncFunctionHandler return async_update_return_; } - void wait_for_update_to_finish() + void wait_for_update_to_finish() const { std::unique_lock lock(async_mtx_); async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); From c2adc425951eca5e2634b8b0d6e44813dd0905be Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 11:13:46 +0200 Subject: [PATCH 22/84] change async_update_ready_ to atomic bool --- .../include/controller_interface/async_function_handler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index c1d2a523e3..aee03fb3ad 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -133,7 +133,7 @@ class AsyncFunctionHandler // Async related variables std::thread thread_; std::atomic_bool async_update_stop_{false}; - bool async_update_ready_{false}; + std::atomic_bool async_update_ready_{false}; std::atomic async_update_return_; std::condition_variable async_update_condition_; std::mutex async_mtx_; From 5ca2e5da0e30aa8ead6b9fa52924d05972131711 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 11:14:47 +0200 Subject: [PATCH 23/84] use also async_update_stop_ also in the conditional wait predicate functon --- .../controller_interface/async_function_handler.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index aee03fb3ad..5d5916dca4 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -113,7 +113,13 @@ class AsyncFunctionHandler while (get_state_function().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { - async_update_condition_.wait(lock, [this] { return async_update_ready_; }); + async_update_condition_.wait( + lock, [this] { return async_update_ready_ || async_update_stop_; }); + if (async_update_stop_) + { + lock.unlock(); + break; + } async_update_return_ = async_function(current_update_time_, current_update_period_); async_update_ready_ = false; lock.unlock(); From c5a00e6d11e3851bfffd7326c7ee9a0c6bb79574 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 11:20:30 +0200 Subject: [PATCH 24/84] Add some utility methods --- .../async_function_handler.hpp | 28 +++++++++++++------ 1 file changed, 19 insertions(+), 9 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 5d5916dca4..ec1031f0b9 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -35,15 +35,7 @@ class AsyncFunctionHandler public: AsyncFunctionHandler() = default; - ~AsyncFunctionHandler() - { - async_update_stop_ = true; - if (thread_.joinable()) - { - async_update_condition_.notify_one(); - thread_.join(); - } - } + ~AsyncFunctionHandler() { preempt_async_update(); } template void init( @@ -84,6 +76,24 @@ class AsyncFunctionHandler return thread_.joinable() && get_state_function_ != nullptr && async_function_ != nullptr; } + bool is_async() const { return thread_.joinable(); } + + bool is_running() const + { + return thread_.joinable() && !async_update_stop_ && + get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + } + + void preempt_async_update() + { + if (is_running()) + { + async_update_stop_ = true; + async_update_condition_.notify_one(); + thread_.join(); + } + } + private: template void initialize_async_update_thread( From b83157d3f32e9749f1d42ed41edc8843dd361c34 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 12 Apr 2024 15:11:32 +0200 Subject: [PATCH 25/84] modify the std::function arguments of async_update method --- .../include/controller_interface/async_function_handler.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index ec1031f0b9..319d9f1081 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -40,7 +40,7 @@ class AsyncFunctionHandler template void init( std::function get_state_function, - std::function async_function) + std::function async_function) { if (thread_.joinable()) { @@ -144,7 +144,7 @@ class AsyncFunctionHandler rclcpp::Duration current_update_period_{0, 0}; std::function get_state_function_; - std::function async_function_; + std::function async_function_; // Async related variables std::thread thread_; From 225c96f35f92c2312740c5a26ed0594511020700 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Apr 2024 23:19:26 +0200 Subject: [PATCH 26/84] Fix the issue with the overidding template names and typo in the std::function --- .../include/controller_interface/async_function_handler.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 319d9f1081..36c34311ec 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -37,7 +37,6 @@ class AsyncFunctionHandler ~AsyncFunctionHandler() { preempt_async_update(); } - template void init( std::function get_state_function, std::function async_function) @@ -50,7 +49,6 @@ class AsyncFunctionHandler async_function_ = async_function; } - template T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) { initialize_async_update_thread(get_state_function_, async_function_); @@ -95,10 +93,9 @@ class AsyncFunctionHandler } private: - template void initialize_async_update_thread( std::function get_state_function, - std::function async_function) + std::function async_function) { // * If the thread is not joinable, create a new thread and call the update function and wait // for it to be triggered again and repeat the same process. From 1f3377c27eed51b68ec1f339b3523a6578fad4ea Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 13 Apr 2024 23:20:17 +0200 Subject: [PATCH 27/84] fix the wait for update to finish method --- .../controller_interface/async_function_handler.hpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 36c34311ec..0c21900359 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -63,10 +63,13 @@ class AsyncFunctionHandler return async_update_return_; } - void wait_for_update_to_finish() const + void wait_for_update_to_finish() { - std::unique_lock lock(async_mtx_); - async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); + if (is_running()) + { + std::unique_lock lock(async_mtx_); + async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); + } } bool is_initialized() const From fd8be6d56cd95e88c9e32dc415adab7585bd9b2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:11:35 +0200 Subject: [PATCH 28/84] Check if the parsed functions are valid or not in the init method --- .../include/controller_interface/async_function_handler.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 0c21900359..c30bc3a570 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -41,6 +41,10 @@ class AsyncFunctionHandler std::function get_state_function, std::function async_function) { + if (get_state_function == nullptr || async_function == nullptr) + { + throw std::runtime_error("AsyncFunctionHandler parsed functions are not valid!"); + } if (thread_.joinable()) { throw std::runtime_error("AsyncFunctionHandler already initialized!"); From 794a8c002a7d2a5d20ab40861c8be8fb37aa3b8c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:12:10 +0200 Subject: [PATCH 29/84] unlock the mutex before notifying the conditional variable --- .../include/controller_interface/async_function_handler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index c30bc3a570..26571d3125 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -62,6 +62,7 @@ class AsyncFunctionHandler async_update_ready_ = true; current_update_time_ = time; current_update_period_ = period; + lock.unlock(); async_update_condition_.notify_one(); } return async_update_return_; From 619f46dc642c30283791c20cb7cd4022d5fca994 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:14:51 +0200 Subject: [PATCH 30/84] Do not check if the thread is joinable in the is_initialized method --- .../include/controller_interface/async_function_handler.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 26571d3125..93e91fd4ce 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -79,7 +79,8 @@ class AsyncFunctionHandler bool is_initialized() const { - return thread_.joinable() && get_state_function_ != nullptr && async_function_ != nullptr; + return get_state_function_ != nullptr && async_function_ != nullptr; + } } bool is_async() const { return thread_.joinable(); } From 98aa3b0488afb0bafd5d77d6c7a3de3364b7a319 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:15:16 +0200 Subject: [PATCH 31/84] added a explicit method to join the async thread --- .../controller_interface/async_function_handler.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 93e91fd4ce..8fec1444d8 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -81,6 +81,13 @@ class AsyncFunctionHandler { return get_state_function_ != nullptr && async_function_ != nullptr; } + + void join_async_update_thread() + { + if (thread_.joinable()) + { + thread_.join(); + } } bool is_async() const { return thread_.joinable(); } From 7f9e47f38de68e0a12eb07211ee740ba0777f801 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:15:36 +0200 Subject: [PATCH 32/84] Add missing includes --- .../include/controller_interface/async_function_handler.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 8fec1444d8..8e0470309b 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -18,6 +18,8 @@ #include #include #include +#include +#include #include #include #include From b5ad67540a0f53df03e96c78449a8426af7ed522 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 10:22:41 +0200 Subject: [PATCH 33/84] change the way the initialization in done --- .../async_function_handler.hpp | 43 ++++++++++--------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 8e0470309b..fee3030a64 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -127,31 +127,32 @@ class AsyncFunctionHandler } if (!thread_.joinable()) { - auto update_fn = [this, get_state_function, async_function]() -> void - { - async_update_stop_ = false; - async_update_ready_ = false; - std::unique_lock lock(async_mtx_); - // \note There might be an concurrency issue with the get_state() call here. This mightn't - // be critical here as the state of the controller is not expected to change during the - // update cycle - while (get_state_function().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && - !async_update_stop_) + async_update_stop_ = false; + async_update_ready_ = false; + async_update_return_ = T(); + thread_ = std::thread( + [this]() -> void { - async_update_condition_.wait( - lock, [this] { return async_update_ready_ || async_update_stop_; }); - if (async_update_stop_) + // \note There might be an concurrency issue with the get|ate() call here. This mightn't + // be critical here as the state of the controller is not expected to change during the + // update cycle + while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + !async_update_stop_) { + std::unique_lock lock(async_mtx_); + async_update_condition_.wait( + lock, [this] { return async_update_ready_ || async_update_stop_; }); + if (async_update_stop_) + { + lock.unlock(); + break; + } + async_update_return_ = async_function_(current_update_time_, current_update_period_); + async_update_ready_ = false; lock.unlock(); - break; + async_update_condition_.notify_one(); } - async_update_return_ = async_function(current_update_time_, current_update_period_); - async_update_ready_ = false; - lock.unlock(); - async_update_condition_.notify_one(); - } - }; - thread_ = std::thread(update_fn); + }); } } From 3cd856fb07ee3d5b0128bbfd628f7c1fed6355ca Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 11:22:17 +0200 Subject: [PATCH 34/84] Added tests for the new async function handler --- controller_interface/CMakeLists.txt | 5 ++ .../test/test_async_function_handler.cpp | 82 +++++++++++++++++++ .../test/test_async_function_handler.hpp | 52 ++++++++++++ 3 files changed, 139 insertions(+) create mode 100644 controller_interface/test/test_async_function_handler.cpp create mode 100644 controller_interface/test/test_async_function_handler.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index 3dc3bc4d0a..a19ff15bac 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -74,6 +74,11 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_sensor sensor_msgs ) + + ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) + target_link_libraries(test_async_function_handler + controller_interface + ) endif() install( diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp new file mode 100644 index 0000000000..f33616200a --- /dev/null +++ b/controller_interface/test/test_async_function_handler.cpp @@ -0,0 +1,82 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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. + +#include "test_async_function_handler.hpp" +#include "gmock/gmock.h" + +namespace controller_interface +{ +TestAsyncFunctionHandler::TestAsyncFunctionHandler() +: state_(rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_async")), + counter_(0), + return_state_(return_type::OK) +{ +} + +TestAsyncFunctionHandler::~TestAsyncFunctionHandler() {} + +void TestAsyncFunctionHandler::initialize() +{ + handler_.init( + std::bind(&TestAsyncFunctionHandler::get_state, this), + std::bind( + &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2)); +} + +return_type TestAsyncFunctionHandler::update( + const rclcpp::Time & time, const rclcpp::Duration & period) +{ + last_callback_time_ = time; + last_callback_period_ = period; + // to simulate some work being done + std::this_thread::sleep_for(std::chrono::microseconds(10)); + counter_++; + return return_state_; +} +const rclcpp_lifecycle::State & TestAsyncFunctionHandler::get_state() const { return state_; } + +} // namespace controller_interface +class AsyncFunctionHandlerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() { rclcpp::shutdown(); } +}; + +TEST_F(AsyncFunctionHandlerTest, check_initialization) +{ + controller_interface::TestAsyncFunctionHandler async_class; + + ASSERT_FALSE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + async_class.initialize(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().preempt_async_update(); + ASSERT_EQ(async_class.get_counter(), 1); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + async_class.get_handler().wait_for_update_to_finish(); +} diff --git a/controller_interface/test/test_async_function_handler.hpp b/controller_interface/test/test_async_function_handler.hpp new file mode 100644 index 0000000000..5d46f30311 --- /dev/null +++ b/controller_interface/test/test_async_function_handler.hpp @@ -0,0 +1,52 @@ +// Copyright 2024 PAL Robotics S.L. +// +// 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 TEST_ASYNC_FUNCTION_HANDLER_HPP_ +#define TEST_ASYNC_FUNCTION_HANDLER_HPP_ + +#include "controller_interface/async_function_handler.hpp" +#include "controller_interface/controller_interface_base.hpp" + +namespace controller_interface +{ +class TestAsyncFunctionHandler +{ +public: + TestAsyncFunctionHandler(); + ~TestAsyncFunctionHandler(); + + void initialize(); + + ros2_control::AsyncFunctionHandler & get_handler() { return handler_; } + + return_type trigger() + { + return handler_.trigger_async_update(last_callback_time_, last_callback_period_); + } + return_type update(const rclcpp::Time & time, const rclcpp::Duration & period); + + const rclcpp_lifecycle::State & get_state() const; + + int get_counter() const { return counter_; } + +private: + rclcpp_lifecycle::State state_; + rclcpp::Time last_callback_time_; + rclcpp::Duration last_callback_period_{0, 0}; + int counter_; + return_type return_state_; + ros2_control::AsyncFunctionHandler handler_; +}; +} // namespace controller_interface +#endif // TEST_ASYNC_FUNCTION_HANDLER_HPP_ From f88fe2057417a9da956a74e96458b89f886afacd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 12:14:42 +0200 Subject: [PATCH 35/84] extend the tests with another loop --- .../test/test_async_function_handler.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index f33616200a..e6dd078ee7 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -74,8 +74,15 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); async_class.get_handler().wait_for_update_to_finish(); - async_class.get_handler().preempt_async_update(); ASSERT_EQ(async_class.get_counter(), 1); + // std::this_thread::sleep_for(std::chrono::microseconds(1)); + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().preempt_async_update(); + ASSERT_EQ(async_class.get_counter(), 2); ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); async_class.get_handler().wait_for_update_to_finish(); From eb7da6b2d01fd432fd7a495e580cff52dac3e3db Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 12:50:59 +0200 Subject: [PATCH 36/84] Added test case to check for triggering --- .../test/test_async_function_handler.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index e6dd078ee7..21d5d2d209 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -63,6 +63,32 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + // It should not be possible to initialize setting wrong functions + EXPECT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); + + async_class.initialize(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + // Once initialized, it should not be possible to initialize again + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + EXPECT_THROW(async_class.initialize(), std::runtime_error); + // The preempt_async_update is already called with the destructor + // async_class.get_handler().preempt_async_update(); +} + +TEST_F(AsyncFunctionHandlerTest, check_triggering) +{ + controller_interface::TestAsyncFunctionHandler async_class; + + ASSERT_FALSE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_async()); @@ -75,6 +101,8 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_TRUE(async_class.get_handler().is_running()); async_class.get_handler().wait_for_update_to_finish(); ASSERT_EQ(async_class.get_counter(), 1); + + // Trigger one more cycle // std::this_thread::sleep_for(std::chrono::microseconds(1)); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); @@ -83,6 +111,8 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) async_class.get_handler().wait_for_update_to_finish(); async_class.get_handler().preempt_async_update(); ASSERT_EQ(async_class.get_counter(), 2); + + // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); async_class.get_handler().wait_for_update_to_finish(); From b61b8cd42dd9b1309fb9f80a3d8e97a520840f09 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 12:56:19 +0200 Subject: [PATCH 37/84] Also test the case of triggering without initialization --- controller_interface/test/test_async_function_handler.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 21d5d2d209..3cc4171a75 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -88,6 +88,8 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_FALSE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + // It shouldn't be possible to trigger without initialization + EXPECT_THROW(async_class.trigger(), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); From 3fe3d3169efc5ea67f564996eec9f2ad5c4b8a88 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 17:47:41 +0200 Subject: [PATCH 38/84] Add test triggering the handler for several cycles --- .../test/test_async_function_handler.cpp | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 3cc4171a75..a0b83c3da1 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -119,3 +119,30 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_FALSE(async_class.get_handler().is_running()); async_class.get_handler().wait_for_update_to_finish(); } + +TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) +{ + controller_interface::TestAsyncFunctionHandler async_class; + + async_class.initialize(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + for (int i = 1; i < 1e4; i++) + { + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + async_class.get_handler().wait_for_update_to_finish(); + ASSERT_EQ(async_class.get_counter(), i); + std::this_thread::sleep_for(std::chrono::microseconds(1)); + } + async_class.get_handler().preempt_async_update(); + + // now the async update should be preempted + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); +} From 1fb59550c4b0f67b6ae353913494f00dcb182e12 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 18:01:42 +0200 Subject: [PATCH 39/84] Add the test cases of changes in the lifecycle state --- .../async_function_handler.hpp | 2 +- .../test/test_async_function_handler.cpp | 64 +++++++++++++++++++ .../test/test_async_function_handler.hpp | 12 ++++ 3 files changed, 77 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index fee3030a64..c132be7152 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -102,7 +102,7 @@ class AsyncFunctionHandler void preempt_async_update() { - if (is_running()) + if (thread_.joinable()) { async_update_stop_ = true; async_update_condition_.notify_one(); diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index a0b83c3da1..8d1e30b2a4 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -146,3 +146,67 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); } + +TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) +{ + controller_interface::TestAsyncFunctionHandler async_class; + + // Start with a deactivated state + async_class.initialize(); + async_class.deactivate(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + // The thread will start and end immediately when invoked in inactive state + for (size_t i = 0; i < 3; i++) + { + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + async_class.get_handler().join_async_update_thread(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + } + + // Now activate it and launch again + async_class.activate(); + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + for (int i = 1; i < 100; i++) + { + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + async_class.get_handler().wait_for_update_to_finish(); + ASSERT_EQ(async_class.get_counter(), i); + std::this_thread::sleep_for(std::chrono::microseconds(1)); + } + + // Now let's do one more trigger cycle and then change the state to deactivate, then it should end + // the thread once the cycle is finished + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + async_class.deactivate(); + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + async_class.get_handler().join_async_update_thread(); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); + + // Now let's test the case of activating it and then deactivating it when the thread is waiting + // for a trigger to start new update cycle execution + async_class.activate(); + + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); + async_class.get_handler().wait_for_update_to_finish(); + async_class.deactivate(); + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // Now they continue to wait until a new cycle is triggered or the preempt is called + ASSERT_TRUE(async_class.get_handler().is_async()); + + // now the async update should be preempted + async_class.get_handler().preempt_async_update(); + ASSERT_FALSE(async_class.get_handler().is_async()); + ASSERT_FALSE(async_class.get_handler().is_running()); +} diff --git a/controller_interface/test/test_async_function_handler.hpp b/controller_interface/test/test_async_function_handler.hpp index 5d46f30311..27b55dc4ba 100644 --- a/controller_interface/test/test_async_function_handler.hpp +++ b/controller_interface/test/test_async_function_handler.hpp @@ -40,6 +40,18 @@ class TestAsyncFunctionHandler int get_counter() const { return counter_; } + void activate() + { + state_ = + rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state_.label()); + } + + void deactivate() + { + state_ = + rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label()); + } + private: rclcpp_lifecycle::State state_; rclcpp::Time last_callback_time_; From 6952577563129c2bfffcc6c22e16f2052448eec0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 14 Apr 2024 18:20:23 +0200 Subject: [PATCH 40/84] remove is_async method and added is_preempted method + changes to the tests --- .../async_function_handler.hpp | 14 ++++---- .../test/test_async_function_handler.cpp | 35 ++++++++++--------- 2 files changed, 26 insertions(+), 23 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index c132be7152..19d0fc47db 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -86,23 +86,25 @@ class AsyncFunctionHandler void join_async_update_thread() { - if (thread_.joinable()) + if (is_running()) { thread_.join(); } } - bool is_async() const { return thread_.joinable(); } + bool is_running() const { return thread_.joinable(); } - bool is_running() const + bool is_preempted() const { - return thread_.joinable() && !async_update_stop_ && - get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + return ( + async_update_stop_ || + (is_initialized() && + get_state_function_().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); } void preempt_async_update() { - if (thread_.joinable()) + if (is_running()) { async_update_stop_ = true; async_update_condition_.notify_one(); diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 8d1e30b2a4..a622cd6054 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -60,22 +60,22 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) controller_interface::TestAsyncFunctionHandler async_class; ASSERT_FALSE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); // It should not be possible to initialize setting wrong functions EXPECT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); // Once initialized, it should not be possible to initialize again ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); EXPECT_THROW(async_class.initialize(), std::runtime_error); // The preempt_async_update is already called with the destructor // async_class.get_handler().preempt_async_update(); @@ -86,21 +86,21 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) controller_interface::TestAsyncFunctionHandler async_class; ASSERT_FALSE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); // It shouldn't be possible to trigger without initialization EXPECT_THROW(async_class.trigger(), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); ASSERT_EQ(async_class.get_counter(), 1); @@ -108,15 +108,15 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) // std::this_thread::sleep_for(std::chrono::microseconds(1)); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); async_class.get_handler().preempt_async_update(); ASSERT_EQ(async_class.get_counter(), 2); // now the async update should be preempted - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); } @@ -126,16 +126,16 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 1e4; i++) { ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); @@ -143,8 +143,8 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) async_class.get_handler().preempt_async_update(); // now the async update should be preempted - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); } TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) @@ -155,8 +155,8 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.initialize(); async_class.deactivate(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); // The thread will start and end immediately when invoked in inactive state for (size_t i = 0; i < 3; i++) @@ -165,8 +165,8 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); async_class.get_handler().join_async_update_thread(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); } // Now activate it and launch again @@ -176,8 +176,8 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) { ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_async()); ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); @@ -190,8 +190,8 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); async_class.get_handler().join_async_update_thread(); ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); // Now let's test the case of activating it and then deactivating it when the thread is waiting // for a trigger to start new update cycle execution @@ -203,10 +203,11 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.deactivate(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // Now they continue to wait until a new cycle is triggered or the preempt is called - ASSERT_TRUE(async_class.get_handler().is_async()); + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); // now the async update should be preempted async_class.get_handler().preempt_async_update(); - ASSERT_FALSE(async_class.get_handler().is_async()); ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_preempted()); } From b3975db21d698b24579a17542408a9375c5fad99 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 10:16:06 +0200 Subject: [PATCH 41/84] Use the AsyncFunctionHandler instead of implementing in the controller_interface --- .../controller_interface_base.hpp | 36 +----------- .../src/controller_interface_base.cpp | 55 ++++--------------- 2 files changed, 13 insertions(+), 78 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 126311087c..66c6a0e853 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -15,11 +15,11 @@ #ifndef CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ #define CONTROLLER_INTERFACE__CONTROLLER_INTERFACE_BASE_HPP_ -#include #include #include #include +#include "controller_interface/async_function_handler.hpp" #include "controller_interface/visibility_control.h" #include "hardware_interface/handle.hpp" @@ -71,15 +71,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy ControllerInterfaceBase() = default; CONTROLLER_INTERFACE_PUBLIC - virtual ~ControllerInterfaceBase() - { - async_update_stop_ = true; - if (thread_.joinable()) - { - async_update_condition_.notify_one(); - thread_.join(); - } - } + virtual ~ControllerInterfaceBase() = default; /// Get configuration for controller's required command interfaces. /** @@ -280,35 +272,13 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; - /// A method to wait until the controller update method is done. - CONTROLLER_INTERFACE_PUBLIC - void wait_for_update_to_finish() - { - if (is_async()) - { - std::unique_lock lock(async_mtx_); - async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); - } - } - protected: std::vector command_interfaces_; std::vector state_interfaces_; private: - void initialize_async_update_thread(); - std::shared_ptr node_; - rclcpp::Time current_update_time_; - rclcpp::Duration current_update_period_{0, 0}; - - // Async related variables - std::thread thread_; - std::atomic_bool async_update_stop_{false}; - bool async_update_ready_{false}; - std::atomic async_update_return_{return_type::OK}; - std::condition_variable async_update_condition_; - std::mutex async_mtx_; + std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index e8446d1815..a7221b18d8 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -88,6 +88,14 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() update_rate_ = static_cast(get_node()->get_parameter("update_rate").as_int()); is_async_ = get_node()->get_parameter("is_async").as_bool(); } + if (is_async_) + { + async_handler_ = std::make_unique>(); + async_handler_->init( + std::bind(&ControllerInterfaceBase::get_state, this), + std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); + } return get_node()->configure(); } @@ -116,22 +124,11 @@ return_type ControllerInterfaceBase::trigger_update( { if (is_async()) { - initialize_async_update_thread(); - std::unique_lock lock(async_mtx_, std::try_to_lock); - if (!async_update_ready_ && lock.owns_lock()) - { - async_update_ready_ = true; - current_update_time_ = time; - current_update_period_ = period; - async_update_condition_.notify_one(); - } - return async_update_return_; + return async_handler_->trigger_async_update(time, period); } else { - current_update_time_ = time; - current_update_period_ = period; - return update(current_update_time_, current_update_period_); + return update(time, period); } return return_type::OK; } @@ -160,36 +157,4 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } -void ControllerInterfaceBase::initialize_async_update_thread() -{ - // * If the thread is not joinable, create a new thread and call the update function and wait - // for it to be triggered again and repeat the same process. - // * If the thread is joinable, check if the current update cycle is finished. If so, then - // trigger a new update cycle. - // * The controller managr is responsible for triggering the update cycle and maintaining the - // controller's update rate and should be acting as a scheduler. - if (is_async() && !thread_.joinable()) - { - auto update_fn = [this]() -> void - { - async_update_stop_ = false; - async_update_ready_ = false; - std::unique_lock lock(async_mtx_); - // \note There might be an concurrency issue with the get_state() call here. This mightn't be - // critical here as the state of the controller is not expected to change during the update - // cycle - while (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && - !async_update_stop_) - { - async_update_condition_.wait(lock, [this] { return async_update_ready_; }); - async_update_return_ = update(current_update_time_, current_update_period_); - async_update_ready_ = false; - lock.unlock(); - async_update_condition_.notify_one(); - } - }; - thread_ = std::thread(update_fn); - } -} - } // namespace controller_interface From 47d59284cbafa68c256f4507ebece11c150f59e2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 16:42:06 +0200 Subject: [PATCH 42/84] Add documentation to the methods --- .../async_function_handler.hpp | 66 +++++++++++++++++-- 1 file changed, 59 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 19d0fc47db..b62f607b2e 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -31,6 +31,11 @@ namespace ros2_control { +/** + * @brief Class to handle asynchronous function calls. + * AsyncFunctionHandler is a class that allows the user to have a ascynchronous call to the parsed + * method, when the lifecycyle state is in the ACTIVE state + */ template class AsyncFunctionHandler { @@ -39,6 +44,13 @@ class AsyncFunctionHandler ~AsyncFunctionHandler() { preempt_async_update(); } + /// Initialize the AsyncFunctionHandler with the get_state_function and async_function + /** + * @param get_state_function Function that returns the current lifecycle state + * @param async_function Function that will be called asynchronously + * If the AsyncFunctionHandler is already initialized and is running, it will throw a runtime + * error. If the parsed functions are not valid, it will throw a runtime error. + */ void init( std::function get_state_function, std::function async_function) @@ -55,6 +67,19 @@ class AsyncFunctionHandler async_function_ = async_function; } + /// Triggers the async update method cycle + /** + * @param time Current time + * @param period Current period + * @return The return value of the async function + * If the AsyncFunctionHandler is not initialized properly, it will throw a runtime error. + * If the async update method is waiting for the trigger, it will notify the async thread to start + * the update cycle. Check if the current update cycle is finished. If so, then trigger a new + * update cycle. If the async update method is still running, it will return the last return value + * of the async function. \note In the case of controllers, The controller manager is responsible + * for triggering and maintaining the controller's update rate, as it should be only acting as a + * scheduler. Same applies to the resource manager when handling the hardware components. + */ T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) { initialize_async_update_thread(get_state_function_, async_function_); @@ -70,6 +95,10 @@ class AsyncFunctionHandler return async_update_return_; } + /// Waits until the async update finishes the current cycle + /** + * If the async method is running, it will wait for the current async method call to finish. + */ void wait_for_update_to_finish() { if (is_running()) @@ -79,11 +108,19 @@ class AsyncFunctionHandler } } + /// Check if the AsyncFunctionHandler is initialized + /** + * @return True if the AsyncFunctionHandler is initialized, false otherwise + */ bool is_initialized() const { return get_state_function_ != nullptr && async_function_ != nullptr; } + /// Join the async update thread + /** + * If the async method is running, it will join the async thread. + */ void join_async_update_thread() { if (is_running()) @@ -92,8 +129,16 @@ class AsyncFunctionHandler } } + /// Check if the async update thread is running + /** + * @return True if the async update thread is running, false otherwise + */ bool is_running() const { return thread_.joinable(); } + /// Check if the async update is preempted + /** + * @return True if the async update is preempted, false otherwise + */ bool is_preempted() const { return ( @@ -102,6 +147,11 @@ class AsyncFunctionHandler get_state_function_().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); } + /// Preempt the async update + /** + * If the async method is running, it will notify the async thread to stop and then joins the + * async thread. + */ void preempt_async_update() { if (is_running()) @@ -113,16 +163,18 @@ class AsyncFunctionHandler } private: + /// Initialize the async update thread + /** + * @param get_state_function Function that returns the current lifecycle state + * @param async_function Function that will be called asynchronously + * If the async update thread is not running, it will start the async update thread. + * If the async update thread is already configuredand running, does nothing and return + * immediately. + */ void initialize_async_update_thread( std::function get_state_function, std::function async_function) { - // * If the thread is not joinable, create a new thread and call the update function and wait - // for it to be triggered again and repeat the same process. - // * If the thread is joinable, check if the current update cycle is finished. If so, then - // trigger a new update cycle. - // * The controller managr is responsible for triggering the update cycle and maintaining the - // controller's update rate and should be acting as a scheduler. if (get_state_function_ == nullptr || async_function_ == nullptr) { throw std::runtime_error("AsyncFunctionHandler not initialized properly!"); @@ -135,7 +187,7 @@ class AsyncFunctionHandler thread_ = std::thread( [this]() -> void { - // \note There might be an concurrency issue with the get|ate() call here. This mightn't + // \note There might be an concurrency issue with the get_state() call here. This mightn't // be critical here as the state of the controller is not expected to change during the // update cycle while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && From d86be02dbcf5d02091c6c232b2fdd708dee8d053 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 19:05:45 +0200 Subject: [PATCH 43/84] remove the arguments to initialize_async_update_thread method --- .../include/controller_interface/async_function_handler.hpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index b62f607b2e..e7297ab0af 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -165,15 +165,11 @@ class AsyncFunctionHandler private: /// Initialize the async update thread /** - * @param get_state_function Function that returns the current lifecycle state - * @param async_function Function that will be called asynchronously * If the async update thread is not running, it will start the async update thread. * If the async update thread is already configuredand running, does nothing and return * immediately. */ - void initialize_async_update_thread( - std::function get_state_function, - std::function async_function) + void initialize_async_update_thread() { if (get_state_function_ == nullptr || async_function_ == nullptr) { From 76e3f30cee6e282360117ae9f17391bec09cfdd8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 19:06:29 +0200 Subject: [PATCH 44/84] simplify the is_initialized method check Co-authored-by: Jordan Palacios --- .../include/controller_interface/async_function_handler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index e7297ab0af..ebdd6b6146 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -114,7 +114,7 @@ class AsyncFunctionHandler */ bool is_initialized() const { - return get_state_function_ != nullptr && async_function_ != nullptr; + return get_state_function_ && async_function_; } /// Join the async update thread From f1634bcf4ce9c4ce656dd516e290cc84271910a0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 19:09:51 +0200 Subject: [PATCH 45/84] rename preempt_async_update to stop_async_update --- .../controller_interface/async_function_handler.hpp | 11 ++++------- .../test/test_async_function_handler.cpp | 6 +++--- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index ebdd6b6146..3ced4344f7 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -42,7 +42,7 @@ class AsyncFunctionHandler public: AsyncFunctionHandler() = default; - ~AsyncFunctionHandler() { preempt_async_update(); } + ~AsyncFunctionHandler() { stop_async_update(); } /// Initialize the AsyncFunctionHandler with the get_state_function and async_function /** @@ -82,7 +82,7 @@ class AsyncFunctionHandler */ T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) { - initialize_async_update_thread(get_state_function_, async_function_); + initialize_async_update_thread(); std::unique_lock lock(async_mtx_, std::try_to_lock); if (!async_update_ready_ && lock.owns_lock()) { @@ -112,10 +112,7 @@ class AsyncFunctionHandler /** * @return True if the AsyncFunctionHandler is initialized, false otherwise */ - bool is_initialized() const - { - return get_state_function_ && async_function_; - } + bool is_initialized() const { return get_state_function_ && async_function_; } /// Join the async update thread /** @@ -152,7 +149,7 @@ class AsyncFunctionHandler * If the async method is running, it will notify the async thread to stop and then joins the * async thread. */ - void preempt_async_update() + void stop_async_update() { if (is_running()) { diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index a622cd6054..753f2425bb 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -111,7 +111,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_preempted()); async_class.get_handler().wait_for_update_to_finish(); - async_class.get_handler().preempt_async_update(); + async_class.get_handler().stop_async_update(); ASSERT_EQ(async_class.get_counter(), 2); // now the async update should be preempted @@ -140,7 +140,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); } - async_class.get_handler().preempt_async_update(); + async_class.get_handler().stop_async_update(); // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); @@ -207,7 +207,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_TRUE(async_class.get_handler().is_preempted()); // now the async update should be preempted - async_class.get_handler().preempt_async_update(); + async_class.get_handler().stop_async_update(); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_preempted()); } From c094f81753b028d67d4aac54f7c2ce7fc0908ab8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 21:42:09 +0200 Subject: [PATCH 46/84] Separate the exceptions for the parsed methods --- .../controller_interface/async_function_handler.hpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 3ced4344f7..b162f34dba 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -55,9 +55,15 @@ class AsyncFunctionHandler std::function get_state_function, std::function async_function) { - if (get_state_function == nullptr || async_function == nullptr) + if (get_state_function == nullptr) { - throw std::runtime_error("AsyncFunctionHandler parsed functions are not valid!"); + throw std::runtime_error( + "AsyncFunctionHandler: parsed function to get the lifecycle state is not valid!"); + } + if (async_function == nullptr) + { + throw std::runtime_error( + "AsyncFunctionHandler: parsed function to call asynchronously is not valid!"); } if (thread_.joinable()) { From 88ceb3c3b63301f693cb0ddb617b70b457d17b5e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 21:49:28 +0200 Subject: [PATCH 47/84] change the exception inside the initialize_async_update_thread --- .../include/controller_interface/async_function_handler.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index b162f34dba..11a65c7501 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -174,9 +174,9 @@ class AsyncFunctionHandler */ void initialize_async_update_thread() { - if (get_state_function_ == nullptr || async_function_ == nullptr) + if (!is_initialized()) { - throw std::runtime_error("AsyncFunctionHandler not initialized properly!"); + throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } if (!thread_.joinable()) { From cecb00bd15863775ab5971c05a407f84aa23d683 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 21:59:12 +0200 Subject: [PATCH 48/84] move the test implementations to the cpp file --- .../test/test_async_function_handler.cpp | 19 +++++++++++++++-- .../test/test_async_function_handler.hpp | 21 +++++-------------- 2 files changed, 22 insertions(+), 18 deletions(-) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 753f2425bb..8fcb8407cb 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -24,8 +24,6 @@ TestAsyncFunctionHandler::TestAsyncFunctionHandler() { } -TestAsyncFunctionHandler::~TestAsyncFunctionHandler() {} - void TestAsyncFunctionHandler::initialize() { handler_.init( @@ -34,6 +32,11 @@ void TestAsyncFunctionHandler::initialize() &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2)); } +return_type TestAsyncFunctionHandler::trigger() +{ + return handler_.trigger_async_update(last_callback_time_, last_callback_period_); +} + return_type TestAsyncFunctionHandler::update( const rclcpp::Time & time, const rclcpp::Duration & period) { @@ -46,6 +49,18 @@ return_type TestAsyncFunctionHandler::update( } const rclcpp_lifecycle::State & TestAsyncFunctionHandler::get_state() const { return state_; } +int TestAsyncFunctionHandler::get_counter() const { return counter_; } +void TestAsyncFunctionHandler::activate() +{ + state_ = + rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state_.label()); +} + +void TestAsyncFunctionHandler::deactivate() +{ + state_ = + rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label()); +} } // namespace controller_interface class AsyncFunctionHandlerTest : public ::testing::Test { diff --git a/controller_interface/test/test_async_function_handler.hpp b/controller_interface/test/test_async_function_handler.hpp index 27b55dc4ba..0a414786fa 100644 --- a/controller_interface/test/test_async_function_handler.hpp +++ b/controller_interface/test/test_async_function_handler.hpp @@ -24,33 +24,22 @@ class TestAsyncFunctionHandler { public: TestAsyncFunctionHandler(); - ~TestAsyncFunctionHandler(); void initialize(); ros2_control::AsyncFunctionHandler & get_handler() { return handler_; } - return_type trigger() - { - return handler_.trigger_async_update(last_callback_time_, last_callback_period_); - } + return_type trigger(); + return_type update(const rclcpp::Time & time, const rclcpp::Duration & period); const rclcpp_lifecycle::State & get_state() const; - int get_counter() const { return counter_; } + int get_counter() const; - void activate() - { - state_ = - rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state_.label()); - } + void activate(); - void deactivate() - { - state_ = - rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label()); - } + void deactivate(); private: rclcpp_lifecycle::State state_; From f61e3826a81f55d9e7be4393c547a92fa85f116c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 22:11:59 +0200 Subject: [PATCH 49/84] Remove the unlock when preempted as the lock goes out of scope --- .../include/controller_interface/async_function_handler.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 11a65c7501..4fb85e4445 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -197,7 +197,6 @@ class AsyncFunctionHandler lock, [this] { return async_update_ready_ || async_update_stop_; }); if (async_update_stop_) { - lock.unlock(); break; } async_update_return_ = async_function_(current_update_time_, current_update_period_); From c070ed644dcb4723687688f9c38d36ee7d93b7dd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 22:36:26 +0200 Subject: [PATCH 50/84] rename async_update_ready_ to trigger_in_progress_ --- .../async_function_handler.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 4fb85e4445..f49b191501 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -90,9 +90,9 @@ class AsyncFunctionHandler { initialize_async_update_thread(); std::unique_lock lock(async_mtx_, std::try_to_lock); - if (!async_update_ready_ && lock.owns_lock()) + if (!trigger_in_progress_ && lock.owns_lock()) { - async_update_ready_ = true; + trigger_in_progress_ = true; current_update_time_ = time; current_update_period_ = period; lock.unlock(); @@ -110,7 +110,7 @@ class AsyncFunctionHandler if (is_running()) { std::unique_lock lock(async_mtx_); - async_update_condition_.wait(lock, [this] { return !async_update_ready_; }); + async_update_condition_.wait(lock, [this] { return !trigger_in_progress_; }); } } @@ -181,7 +181,7 @@ class AsyncFunctionHandler if (!thread_.joinable()) { async_update_stop_ = false; - async_update_ready_ = false; + trigger_in_progress_ = false; async_update_return_ = T(); thread_ = std::thread( [this]() -> void @@ -194,13 +194,13 @@ class AsyncFunctionHandler { std::unique_lock lock(async_mtx_); async_update_condition_.wait( - lock, [this] { return async_update_ready_ || async_update_stop_; }); + lock, [this] { return trigger_in_progress_ || async_update_stop_; }); if (async_update_stop_) { break; } async_update_return_ = async_function_(current_update_time_, current_update_period_); - async_update_ready_ = false; + trigger_in_progress_ = false; lock.unlock(); async_update_condition_.notify_one(); } @@ -217,7 +217,7 @@ class AsyncFunctionHandler // Async related variables std::thread thread_; std::atomic_bool async_update_stop_{false}; - std::atomic_bool async_update_ready_{false}; + std::atomic_bool trigger_in_progress_{false}; std::atomic async_update_return_; std::condition_variable async_update_condition_; std::mutex async_mtx_; From fff989e944fb1795228cf7fe793c31453d9ed0fd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 22:43:07 +0200 Subject: [PATCH 51/84] update the docs --- .../async_function_handler.hpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index f49b191501..538e52e471 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -49,7 +49,8 @@ class AsyncFunctionHandler * @param get_state_function Function that returns the current lifecycle state * @param async_function Function that will be called asynchronously * If the AsyncFunctionHandler is already initialized and is running, it will throw a runtime - * error. If the parsed functions are not valid, it will throw a runtime error. + * error. + * If the parsed functions are not valid, it will throw a runtime error. */ void init( std::function get_state_function, @@ -80,9 +81,11 @@ class AsyncFunctionHandler * @return The return value of the async function * If the AsyncFunctionHandler is not initialized properly, it will throw a runtime error. * If the async update method is waiting for the trigger, it will notify the async thread to start - * the update cycle. Check if the current update cycle is finished. If so, then trigger a new - * update cycle. If the async update method is still running, it will return the last return value - * of the async function. \note In the case of controllers, The controller manager is responsible + * the update cycle. + * If the async update method is still running, it will return the last return value of the async + * function. + * + * \note In the case of controllers, The controller manager is responsible * for triggering and maintaining the controller's update rate, as it should be only acting as a * scheduler. Same applies to the resource manager when handling the hardware components. */ @@ -123,6 +126,7 @@ class AsyncFunctionHandler /// Join the async update thread /** * If the async method is running, it will join the async thread. + * If the async method is not running, it will return immediately. */ void join_async_update_thread() { @@ -150,7 +154,7 @@ class AsyncFunctionHandler get_state_function_().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); } - /// Preempt the async update + /// Stops the async update thread /** * If the async method is running, it will notify the async thread to stop and then joins the * async thread. @@ -169,7 +173,7 @@ class AsyncFunctionHandler /// Initialize the async update thread /** * If the async update thread is not running, it will start the async update thread. - * If the async update thread is already configuredand running, does nothing and return + * If the async update thread is already configured and running, does nothing and return * immediately. */ void initialize_async_update_thread() From 71f7ad961e3d7df47ccb5e1cd0e631c63de1ebed Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 22:52:16 +0200 Subject: [PATCH 52/84] rename the method wait_for_update_to_finish to wait_for_trigger_cycle_to_finish for better meaning --- .../controller_interface/async_function_handler.hpp | 4 ++-- .../test/test_async_function_handler.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 538e52e471..bfc2c40091 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -104,11 +104,11 @@ class AsyncFunctionHandler return async_update_return_; } - /// Waits until the async update finishes the current cycle + /// Waits until the current async update method cycle to finish /** * If the async method is running, it will wait for the current async method call to finish. */ - void wait_for_update_to_finish() + void wait_for_trigger_cycle_to_finish() { if (is_running()) { diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 8fcb8407cb..f7eea03c42 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -116,7 +116,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_preempted()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), 1); // Trigger one more cycle @@ -125,14 +125,14 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_preempted()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); async_class.get_handler().stop_async_update(); ASSERT_EQ(async_class.get_counter(), 2); // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_preempted()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); } TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) @@ -151,7 +151,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_preempted()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); } @@ -193,7 +193,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_preempted()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); } @@ -214,7 +214,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - async_class.get_handler().wait_for_update_to_finish(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); async_class.deactivate(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // Now they continue to wait until a new cycle is triggered or the preempt is called From fa6a88c17729050728023845d37a25159ff986f9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 23:05:44 +0200 Subject: [PATCH 53/84] Add documentation to the trigger_update method and add it to the controller_manager --- .../controller_interface_base.hpp | 11 +++++++++++ controller_manager/src/controller_manager.cpp | 2 +- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 66c6a0e853..2c8f4f1f42 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -154,6 +154,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0; + /** + * Trigger update method. This method is used by the controller_manager to trigger the update + * method of the controller. + * The method is used to trigger the update method of the controller synchronously or + * asynchronously, based on the controller configuration. + * **The method called in the (real-time) control loop.** + * + * \param[in] time The time at the start of this control loop iteration + * \param[in] period The measured time taken by the last control loop iteration + * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. + */ CONTROLLER_INTERFACE_PUBLIC return_type trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index b4f18dd90e..d4a0b65bbb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2298,7 +2298,7 @@ controller_interface::return_type ControllerManager::update( // Catch exceptions thrown by the controller update function try { - controller_ret = loaded_controller.c->update(time, controller_actual_period); + controller_ret = loaded_controller.c->trigger_update(time, controller_actual_period); } catch (const std::exception & e) { From 91aab1fdcbed64cabed1393570a87cdde309954a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 15 Apr 2024 23:19:30 +0200 Subject: [PATCH 54/84] added a method to stop the async update cycle before deactivating the controller --- .../controller_interface_base.hpp | 15 +++++++++++++++ .../src/controller_interface_base.cpp | 7 +++++++ controller_manager/src/controller_manager.cpp | 2 ++ 3 files changed, 24 insertions(+) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 2c8f4f1f42..f8fa123aa1 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -283,6 +283,21 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; + /// Method to stop any running async update cycle after finishing the current cycle. + /** + * Method to stop any running async update cycle after finishing the current cycle. This is needed + * to be called before deactivating the controller by the controller_manager, so that the + * interfaces still exist when the controller finishes its cycle and then it's exits. + * + * **The method is not real-time safe and shouldn't be called in the control loop.** + * + * If the controller is running in async mode, the method will stop after the current update cycle + * execution. + * If the controller is not running in async mode, the method will do nothing. + */ + CONTROLLER_INTERFACE_PUBLIC + void stop_async_update_cycle(); + protected: std::vector command_interfaces_; std::vector state_interfaces_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a7221b18d8..8b716d6c64 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -157,4 +157,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } +void ControllerInterfaceBase::stop_async_update_cycle() +{ + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->stop_async_update(); + } +} } // namespace controller_interface diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d4a0b65bbb..c4ac49e313 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1490,6 +1490,8 @@ void ControllerManager::deactivate_controllers( { try { + // This is needed by the async controllers to finish their current cycle + controller->stop_async_update_cycle(); const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); From 6ad0c1bb1df544897fda77494dc668b17042b624 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 12:45:44 +0200 Subject: [PATCH 55/84] return std::pair to check if the trigger is successful or not --- .../async_function_handler.hpp | 9 +++++++-- .../controller_interface_base.hpp | 4 +++- .../src/controller_interface_base.cpp | 6 ++---- .../test/test_async_function_handler.cpp | 2 +- controller_manager/src/controller_manager.cpp | 15 ++++++++++++--- 5 files changed, 25 insertions(+), 11 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index bfc2c40091..eb39eb26d2 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -89,10 +90,12 @@ class AsyncFunctionHandler * for triggering and maintaining the controller's update rate, as it should be only acting as a * scheduler. Same applies to the resource manager when handling the hardware components. */ - T trigger_async_update(const rclcpp::Time & time, const rclcpp::Duration & period) + std::pair trigger_async_update( + const rclcpp::Time & time, const rclcpp::Duration & period) { initialize_async_update_thread(); std::unique_lock lock(async_mtx_, std::try_to_lock); + bool trigger_status = false; if (!trigger_in_progress_ && lock.owns_lock()) { trigger_in_progress_ = true; @@ -100,8 +103,10 @@ class AsyncFunctionHandler current_update_period_ = period; lock.unlock(); async_update_condition_.notify_one(); + trigger_status = true; } - return async_update_return_; + const T return_value = async_update_return_; + return std::make_pair(trigger_status, return_value); } /// Waits until the current async update method cycle to finish diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index f8fa123aa1..958e3eb9db 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -17,6 +17,7 @@ #include #include +#include #include #include "controller_interface/async_function_handler.hpp" @@ -166,7 +167,8 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy * \returns return_type::OK if update is successfully, otherwise return_type::ERROR. */ CONTROLLER_INTERFACE_PUBLIC - return_type trigger_update(const rclcpp::Time & time, const rclcpp::Duration & period); + std::pair trigger_update( + const rclcpp::Time & time, const rclcpp::Duration & period); CONTROLLER_INTERFACE_PUBLIC std::shared_ptr get_node(); diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 8b716d6c64..f1d21c5041 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -16,7 +16,6 @@ #include #include -#include #include #include "lifecycle_msgs/msg/state.hpp" @@ -119,7 +118,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::get_lifecycle_state() c return node_->get_current_state(); } -return_type ControllerInterfaceBase::trigger_update( +std::pair ControllerInterfaceBase::trigger_update( const rclcpp::Time & time, const rclcpp::Duration & period) { if (is_async()) @@ -128,9 +127,8 @@ return_type ControllerInterfaceBase::trigger_update( } else { - return update(time, period); + return std::make_pair(true, update(time, period)); } - return return_type::OK; } std::shared_ptr ControllerInterfaceBase::get_node() diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index f7eea03c42..8a111c3d12 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -34,7 +34,7 @@ void TestAsyncFunctionHandler::initialize() return_type TestAsyncFunctionHandler::trigger() { - return handler_.trigger_async_update(last_callback_time_, last_callback_period_); + return handler_.trigger_async_update(last_callback_time_, last_callback_period_).second; } return_type TestAsyncFunctionHandler::update( diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index c4ac49e313..d4e37da7bf 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2263,7 +2263,7 @@ controller_interface::return_type ControllerManager::update( { // TODO(v-lopez) we could cache this information // https://github.com/ros-controls/ros2_control/issues/153 - if (!loaded_controller.c->is_async() && is_controller_active(*loaded_controller.c)) + if (is_controller_active(*loaded_controller.c)) { const auto controller_update_rate = loaded_controller.c->get_update_rate(); const bool run_controller_at_cm_rate = (controller_update_rate >= update_rate_); @@ -2318,11 +2318,20 @@ controller_interface::return_type ControllerManager::update( } *loaded_controller.next_update_cycle_time += controller_period; + if (!controller_ret.first) + { + RCLCPP_WARN( + get_logger(), + "The controller '%s' missed an update cycle at time : '%f', will trigger next update " + "cycle at around : '%f'", + loaded_controller.info.name.c_str(), time.seconds(), + loaded_controller.next_update_cycle_time->seconds()); + } - if (controller_ret != controller_interface::return_type::OK) + if (controller_ret.second != controller_interface::return_type::OK) { failed_controllers_list.push_back(loaded_controller.info.name); - ret = controller_ret; + ret = controller_ret.second; } } } From d2c66920b8733375088dbeeec5fc639ef0a5fee7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Apr 2024 17:20:34 +0200 Subject: [PATCH 56/84] update the runtime error throw message --- .../include/controller_interface/async_function_handler.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index eb39eb26d2..486db3c25a 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -69,7 +69,9 @@ class AsyncFunctionHandler } if (thread_.joinable()) { - throw std::runtime_error("AsyncFunctionHandler already initialized!"); + throw std::runtime_error( + "AsyncFunctionHandler: Cannot reinitialize while the thread is " + "running. Please stop the async update first!"); } get_state_function_ = get_state_function; async_function_ = async_function; From c4b9f82e6b475740bfe64b5a4ecd1d6e73a97d93 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 15:18:09 +0200 Subject: [PATCH 57/84] rename is_preempted to is_stopped for the context of the threads --- .../async_function_handler.hpp | 6 ++-- .../test/test_async_function_handler.cpp | 34 +++++++++---------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 486db3c25a..c2cbc7ac2c 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -149,11 +149,11 @@ class AsyncFunctionHandler */ bool is_running() const { return thread_.joinable(); } - /// Check if the async update is preempted + /// Check if the async update is triggered to stop the cycle /** - * @return True if the async update is preempted, false otherwise + * @return True if the async update is stopped, false otherwise */ - bool is_preempted() const + bool is_stopped() const { return ( async_update_stop_ || diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 8a111c3d12..639b4073b2 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -76,7 +76,7 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_FALSE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); // It should not be possible to initialize setting wrong functions EXPECT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); @@ -84,13 +84,13 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); // Once initialized, it should not be possible to initialize again ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); EXPECT_THROW(async_class.initialize(), std::runtime_error); // The preempt_async_update is already called with the destructor // async_class.get_handler().preempt_async_update(); @@ -102,20 +102,20 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_FALSE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); // It shouldn't be possible to trigger without initialization EXPECT_THROW(async_class.trigger(), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), 1); @@ -124,14 +124,14 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); async_class.get_handler().stop_async_update(); ASSERT_EQ(async_class.get_counter(), 2); // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); } @@ -142,7 +142,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 1e4; i++) @@ -150,7 +150,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); @@ -159,7 +159,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); } TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) @@ -171,7 +171,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.deactivate(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); // The thread will start and end immediately when invoked in inactive state for (size_t i = 0; i < 3; i++) @@ -181,7 +181,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.get_handler().join_async_update_thread(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); } // Now activate it and launch again @@ -192,7 +192,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_preempted()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); @@ -206,7 +206,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.get_handler().join_async_update_thread(); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); // Now let's test the case of activating it and then deactivating it when the thread is waiting // for a trigger to start new update cycle execution @@ -219,10 +219,10 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // Now they continue to wait until a new cycle is triggered or the preempt is called ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); // now the async update should be preempted async_class.get_handler().stop_async_update(); ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_preempted()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); } From 48b2c2ffcd589de4171cfe16688fb09a9edcb592 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 15:22:05 +0200 Subject: [PATCH 58/84] change EXPECT_THROW to ASSERT_THROW --- controller_interface/test/test_async_function_handler.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 639b4073b2..5fadac7378 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -79,7 +79,7 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_FALSE(async_class.get_handler().is_stopped()); // It should not be possible to initialize setting wrong functions - EXPECT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); + ASSERT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); @@ -91,7 +91,7 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); - EXPECT_THROW(async_class.initialize(), std::runtime_error); + ASSERT_THROW(async_class.initialize(), std::runtime_error); // The preempt_async_update is already called with the destructor // async_class.get_handler().preempt_async_update(); } @@ -104,7 +104,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); // It shouldn't be possible to trigger without initialization - EXPECT_THROW(async_class.trigger(), std::runtime_error); + ASSERT_THROW(async_class.trigger(), std::runtime_error); async_class.initialize(); ASSERT_TRUE(async_class.get_handler().is_initialized()); From a62da78cd6a8e92ccaeb1a628bf6c729bfd3e2f7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 15:22:35 +0200 Subject: [PATCH 59/84] Update the documentation of the init return entity --- .../include/controller_interface/async_function_handler.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index c2cbc7ac2c..07c8217c23 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -81,7 +81,8 @@ class AsyncFunctionHandler /** * @param time Current time * @param period Current period - * @return The return value of the async function + * @return A pair with the first element being a boolean indicating if the async update method was + * triggered and the second element being the last return value of the async function. * If the AsyncFunctionHandler is not initialized properly, it will throw a runtime error. * If the async update method is waiting for the trigger, it will notify the async thread to start * the update cycle. From f8a93ff217448d76140b0b8d1e366009f395a614 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 15:23:12 +0200 Subject: [PATCH 60/84] Fix the doc of stop_async_update_cycle Co-authored-by: Jordan Palacios --- .../include/controller_interface/controller_interface_base.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 958e3eb9db..8d86dd7b29 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -285,7 +285,6 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy CONTROLLER_INTERFACE_PUBLIC virtual bool is_in_chained_mode() const = 0; - /// Method to stop any running async update cycle after finishing the current cycle. /** * Method to stop any running async update cycle after finishing the current cycle. This is needed * to be called before deactivating the controller by the controller_manager, so that the From 5d4b4e2ffa4ea3bb4926c729e70ef6528ed9bb4d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 15:41:22 +0200 Subject: [PATCH 61/84] add minor information --- .../include/controller_interface/async_function_handler.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 07c8217c23..9001892524 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +/// \author Sai Kishor Kothakota + #ifndef CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ #define CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ From 20bc5d13b097a022b7076ee10008aa6c697f5dc0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 18 Apr 2024 16:23:05 +0200 Subject: [PATCH 62/84] add missing functional include --- .../include/controller_interface/async_function_handler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 9001892524..838010d028 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -19,6 +19,7 @@ #include #include +#include #include #include #include From 600da5cd0e74ad3fba32644f8c926ee6ff8f5017 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 00:04:10 +0200 Subject: [PATCH 63/84] check if lock is owned or not before in the condition Co-authored-by: atzaros <128592691+atzaros@users.noreply.github.com> --- .../include/controller_interface/async_function_handler.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 838010d028..1173f485bf 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -102,7 +102,7 @@ class AsyncFunctionHandler initialize_async_update_thread(); std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; - if (!trigger_in_progress_ && lock.owns_lock()) + if (lock.owns_lock() && !trigger_in_progress_) { trigger_in_progress_ = true; current_update_time_ = time; From 7f2aa644a4b9a45c365013285808544c62d81b2a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 00:11:40 +0200 Subject: [PATCH 64/84] Use separated arguments for better clarity --- controller_manager/src/controller_manager.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d4e37da7bf..38fd60396c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2297,10 +2297,12 @@ controller_interface::return_type ControllerManager::update( const auto controller_actual_period = (time - *loaded_controller.next_update_cycle_time) + controller_period; auto controller_ret = controller_interface::return_type::OK; + bool trigger_status = true; // Catch exceptions thrown by the controller update function try { - controller_ret = loaded_controller.c->trigger_update(time, controller_actual_period); + std::tie(trigger_status, controller_ret) = + loaded_controller.c->trigger_update(time, controller_actual_period); } catch (const std::exception & e) { @@ -2318,7 +2320,7 @@ controller_interface::return_type ControllerManager::update( } *loaded_controller.next_update_cycle_time += controller_period; - if (!controller_ret.first) + if (!trigger_status) { RCLCPP_WARN( get_logger(), @@ -2328,10 +2330,10 @@ controller_interface::return_type ControllerManager::update( loaded_controller.next_update_cycle_time->seconds()); } - if (controller_ret.second != controller_interface::return_type::OK) + if (controller_ret != controller_interface::return_type::OK) { failed_controllers_list.push_back(loaded_controller.info.name); - ret = controller_ret.second; + ret = controller_ret; } } } From 679fbf9e0f36baed60180db449b26b0489e554f7 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 09:26:25 +0200 Subject: [PATCH 65/84] Move the lock within a scope to avoid exclusive unlock within the thread --- .../async_function_handler.hpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 1173f485bf..cdb189da45 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -207,16 +207,17 @@ class AsyncFunctionHandler while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && !async_update_stop_) { - std::unique_lock lock(async_mtx_); - async_update_condition_.wait( - lock, [this] { return trigger_in_progress_ || async_update_stop_; }); - if (async_update_stop_) { - break; + std::unique_lock lock(async_mtx_); + async_update_condition_.wait( + lock, [this] { return trigger_in_progress_ || async_update_stop_; }); + if (async_update_stop_) + { + break; + } + async_update_return_ = async_function_(current_update_time_, current_update_period_); + trigger_in_progress_ = false; } - async_update_return_ = async_function_(current_update_time_, current_update_period_); - trigger_in_progress_ = false; - lock.unlock(); async_update_condition_.notify_one(); } }); From d1ac8dffac3dbca19977d5f50825f3b311dfb9e4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 09:33:51 +0200 Subject: [PATCH 66/84] notify other waiting threads before stopping --- .../include/controller_interface/async_function_handler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index cdb189da45..7311bd633b 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -213,6 +213,7 @@ class AsyncFunctionHandler lock, [this] { return trigger_in_progress_ || async_update_stop_; }); if (async_update_stop_) { + async_update_condition_.notify_one(); break; } async_update_return_ = async_function_(current_update_time_, current_update_period_); From 4e3dbb18f664a51c282affd15282dc45a76f1f86 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 09:57:01 +0200 Subject: [PATCH 67/84] Scope the lock to avoid manual unlock in the trigger_async_update method --- .../controller_interface/async_function_handler.hpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 7311bd633b..544d4b53ef 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -104,10 +104,12 @@ class AsyncFunctionHandler bool trigger_status = false; if (lock.owns_lock() && !trigger_in_progress_) { - trigger_in_progress_ = true; - current_update_time_ = time; - current_update_period_ = period; - lock.unlock(); + { + std::unique_lock scoped_lock(std::move(lock)); + trigger_in_progress_ = true; + current_update_time_ = time; + current_update_period_ = period; + } async_update_condition_.notify_one(); trigger_status = true; } From 30be210267de1f059a26fc6cfc988a768f9d61b3 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 17:49:36 +0200 Subject: [PATCH 68/84] Update async_update_stop_ within the scope of the lock to avoid missing notify --- .../include/controller_interface/async_function_handler.hpp | 5 ++++- controller_interface/test/test_async_function_handler.cpp | 2 +- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 544d4b53ef..21c3186e93 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -176,7 +176,10 @@ class AsyncFunctionHandler { if (is_running()) { - async_update_stop_ = true; + { + std::unique_lock lock(async_mtx_); + async_update_stop_ = true; + } async_update_condition_.notify_one(); thread_.join(); } diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index 5fadac7378..f6ac2d77ca 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -125,7 +125,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); + std::this_thread::sleep_for(std::chrono::microseconds(1)); async_class.get_handler().stop_async_update(); ASSERT_EQ(async_class.get_counter(), 2); From e54e6135c6654543044a0240e8248163df97b282 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 17:53:26 +0200 Subject: [PATCH 69/84] Add new conditional variable to have unexpected behavior when working between threads --- .../controller_interface/async_function_handler.hpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 21c3186e93..46bd83f24c 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -126,7 +126,7 @@ class AsyncFunctionHandler if (is_running()) { std::unique_lock lock(async_mtx_); - async_update_condition_.wait(lock, [this] { return !trigger_in_progress_; }); + cycle_end_condition_.wait(lock, [this] { return !trigger_in_progress_; }); } } @@ -218,13 +218,14 @@ class AsyncFunctionHandler lock, [this] { return trigger_in_progress_ || async_update_stop_; }); if (async_update_stop_) { - async_update_condition_.notify_one(); + trigger_in_progress_ = false; + cycle_end_condition_.notify_one(); break; } async_update_return_ = async_function_(current_update_time_, current_update_period_); trigger_in_progress_ = false; } - async_update_condition_.notify_one(); + cycle_end_condition_.notify_one(); } }); } @@ -242,6 +243,7 @@ class AsyncFunctionHandler std::atomic_bool trigger_in_progress_{false}; std::atomic async_update_return_; std::condition_variable async_update_condition_; + std::condition_variable cycle_end_condition_; std::mutex async_mtx_; }; } // namespace ros2_control From 47f6b028314664265aa7a4ec0589de7e8b51284f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 00:20:32 +0200 Subject: [PATCH 70/84] Changes for the starting thread upon activation --- .../async_function_handler.hpp | 20 ++++++++++++--- .../src/controller_interface_base.cpp | 11 ++++++-- .../test/test_async_function_handler.cpp | 25 +++++++++++-------- 3 files changed, 39 insertions(+), 17 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 46bd83f24c..732addeccd 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -99,7 +99,15 @@ class AsyncFunctionHandler std::pair trigger_async_update( const rclcpp::Time & time, const rclcpp::Duration & period) { - initialize_async_update_thread(); + if (!is_initialized()) + { + throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); + } + if (!is_running()) + { + throw std::runtime_error( + "AsyncFunctionHandler: need to start the async update thread first before triggering!"); + } std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; if (lock.owns_lock() && !trigger_in_progress_) @@ -185,14 +193,13 @@ class AsyncFunctionHandler } } -private: /// Initialize the async update thread /** * If the async update thread is not running, it will start the async update thread. * If the async update thread is already configured and running, does nothing and return * immediately. */ - void initialize_async_update_thread() + void start_async_update_thread() { if (!is_initialized()) { @@ -209,7 +216,9 @@ class AsyncFunctionHandler // \note There might be an concurrency issue with the get_state() call here. This mightn't // be critical here as the state of the controller is not expected to change during the // update cycle - while (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE && + while ((get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || + get_state_function_().id() == + lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) && !async_update_stop_) { { @@ -227,10 +236,13 @@ class AsyncFunctionHandler } cycle_end_condition_.notify_one(); } + trigger_in_progress_ = false; + cycle_end_condition_.notify_one(); }); } } +private: rclcpp::Time current_update_time_; rclcpp::Duration current_update_period_{0, 0}; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index f1d21c5041..2741c67289 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -57,8 +57,15 @@ return_type ControllerInterfaceBase::init( node_->register_on_cleanup( std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); - node_->register_on_activate( - std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); + auto activate_callback = [this](const rclcpp_lifecycle::State & state) + { + if (is_async() && async_handler_) + { + async_handler_->start_async_update_thread(); + } + return on_activate(state); + }; + node_->register_on_activate(activate_callback); node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp index f6ac2d77ca..70cb96370d 100644 --- a/controller_interface/test/test_async_function_handler.cpp +++ b/controller_interface/test/test_async_function_handler.cpp @@ -87,13 +87,12 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) ASSERT_FALSE(async_class.get_handler().is_stopped()); // Once initialized, it should not be possible to initialize again + async_class.get_handler().start_async_update_thread(); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); ASSERT_THROW(async_class.initialize(), std::runtime_error); - // The preempt_async_update is already called with the destructor - // async_class.get_handler().preempt_async_update(); } TEST_F(AsyncFunctionHandlerTest, check_triggering) @@ -110,6 +109,9 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); + // It shouldn't be possible to trigger without starting the thread + ASSERT_THROW(async_class.trigger(), std::runtime_error); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); @@ -143,6 +145,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 1e4; i++) @@ -172,20 +175,19 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_stopped()); + async_class.get_handler().start_async_update_thread(); // The thread will start and end immediately when invoked in inactive state - for (size_t i = 0; i < 3; i++) - { - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - async_class.get_handler().join_async_update_thread(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - } + async_class.get_handler().join_async_update_thread(); + EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_THROW(async_class.trigger(), std::runtime_error); + ASSERT_TRUE(async_class.get_handler().is_initialized()); + ASSERT_FALSE(async_class.get_handler().is_running()); + ASSERT_TRUE(async_class.get_handler().is_stopped()); // Now activate it and launch again async_class.activate(); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); for (int i = 1; i < 100; i++) { @@ -211,6 +213,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) // Now let's test the case of activating it and then deactivating it when the thread is waiting // for a trigger to start new update cycle execution async_class.activate(); + async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); From 85257259a1c0cb7f620db39f8a4d6d7bd04c09fa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 22 Apr 2024 22:25:57 +0200 Subject: [PATCH 71/84] remove unused notify at the end of the thread --- .../include/controller_interface/async_function_handler.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index 732addeccd..b32fbdb1f1 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -236,8 +236,6 @@ class AsyncFunctionHandler } cycle_end_condition_.notify_one(); } - trigger_in_progress_ = false; - cycle_end_condition_.notify_one(); }); } } From 4eb74cc971f010df2107d8206e1aaea5023bcf8c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 May 2024 23:16:03 +0200 Subject: [PATCH 72/84] simplify the logic inside the thread Co-authored-by: atzaros <128592691+atzaros@users.noreply.github.com> --- .../controller_interface/async_function_handler.hpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index b32fbdb1f1..e8ec70c0f1 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -225,16 +225,13 @@ class AsyncFunctionHandler std::unique_lock lock(async_mtx_); async_update_condition_.wait( lock, [this] { return trigger_in_progress_ || async_update_stop_; }); - if (async_update_stop_) + if (!async_update_stop_) { - trigger_in_progress_ = false; - cycle_end_condition_.notify_one(); - break; + async_update_return_ = async_function_(current_update_time_, current_update_period_); } - async_update_return_ = async_function_(current_update_time_, current_update_period_); trigger_in_progress_ = false; } - cycle_end_condition_.notify_one(); + cycle_end_condition_.notify_all(); } }); } From a131029b1fd2b47018f9bcd7ecdae9c843bc6dbd Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 May 2024 23:22:29 +0200 Subject: [PATCH 73/84] add pre-commit formatting changes --- .../include/controller_interface/async_function_handler.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp index e8ec70c0f1..42b9a98f29 100644 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ b/controller_interface/include/controller_interface/async_function_handler.hpp @@ -227,7 +227,8 @@ class AsyncFunctionHandler lock, [this] { return trigger_in_progress_ || async_update_stop_; }); if (!async_update_stop_) { - async_update_return_ = async_function_(current_update_time_, current_update_period_); + async_update_return_ = + async_function_(current_update_time_, current_update_period_); } trigger_in_progress_ = false; } From b4b2afa32258ee7227dc90c3d2345e55c2c18fff Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 17 May 2024 23:39:09 +0200 Subject: [PATCH 74/84] remove the AsyncControllerThread integration to replace it with AsyncFunctionHandler --- .../controller_interface/async_controller.hpp | 112 ------------------ .../controller_manager/controller_manager.hpp | 4 - controller_manager/src/controller_manager.cpp | 22 ---- 3 files changed, 138 deletions(-) delete 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 deleted file mode 100644 index 357b3a2ce3..0000000000 --- a/controller_interface/include/controller_interface/async_controller.hpp +++ /dev/null @@ -1,112 +0,0 @@ -// 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_lifecycle_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 7c09377bb9..b7dc34310a 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -22,7 +22,6 @@ #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" @@ -606,9 +605,6 @@ class ControllerManager : public rclcpp::Node }; SwitchParams switch_params_; - - std::unordered_map> - async_controller_threads_; }; } // namespace controller_manager diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 38fd60396c..15774168f3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -585,13 +585,6 @@ controller_interface::return_type ControllerManager::unload_controller( controller_name.c_str()); return controller_interface::return_type::ERROR; } - if (controller.c->is_async()) - { - RCLCPP_DEBUG( - get_logger(), "Removing controller '%s' from the list of async controllers", - controller_name.c_str()); - async_controller_threads_.erase(controller_name); - } RCLCPP_DEBUG(get_logger(), "Cleanup controller"); controller_chain_spec_cleanup(controller_chain_spec_, controller_name); @@ -734,14 +727,6 @@ controller_interface::return_type ControllerManager::configure_controller( 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_)); - } - const auto controller_update_rate = controller->get_update_rate(); const auto cm_update_rate = get_update_rate(); if (controller_update_rate > cm_update_rate) @@ -1718,11 +1703,6 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_exported_state_interfaces_available(controller_name); resource_manager_->make_controller_reference_interfaces_available(controller_name); } - - if (controller->is_async()) - { - async_controller_threads_.at(controller_name)->activate(); - } } } @@ -2480,8 +2460,6 @@ 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(); } From ee64bc735ef93273c7ce946953a889bd0da5a2de Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 12 Jun 2024 13:36:44 +0200 Subject: [PATCH 75/84] move the async function handler to the realtime_tools and integrate from it --- controller_interface/CMakeLists.txt | 6 +- .../async_function_handler.hpp | 259 ------------------ .../controller_interface_base.hpp | 4 +- controller_interface/package.xml | 4 + .../src/controller_interface_base.cpp | 2 +- .../test/test_async_function_handler.cpp | 231 ---------------- .../test/test_async_function_handler.hpp | 53 ---- 7 files changed, 8 insertions(+), 551 deletions(-) delete mode 100644 controller_interface/include/controller_interface/async_function_handler.hpp delete mode 100644 controller_interface/test/test_async_function_handler.cpp delete mode 100644 controller_interface/test/test_async_function_handler.hpp diff --git a/controller_interface/CMakeLists.txt b/controller_interface/CMakeLists.txt index a19ff15bac..cd812b9ff2 100644 --- a/controller_interface/CMakeLists.txt +++ b/controller_interface/CMakeLists.txt @@ -9,6 +9,7 @@ endif() set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface rclcpp_lifecycle + realtime_tools ) find_package(ament_cmake REQUIRED) @@ -74,11 +75,6 @@ if(BUILD_TESTING) ament_target_dependencies(test_imu_sensor sensor_msgs ) - - ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) - target_link_libraries(test_async_function_handler - controller_interface - ) endif() install( diff --git a/controller_interface/include/controller_interface/async_function_handler.hpp b/controller_interface/include/controller_interface/async_function_handler.hpp deleted file mode 100644 index 42b9a98f29..0000000000 --- a/controller_interface/include/controller_interface/async_function_handler.hpp +++ /dev/null @@ -1,259 +0,0 @@ -// Copyright 2024 PAL Robotics S.L. -// -// 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. - -/// \author Sai Kishor Kothakota - -#ifndef CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ -#define CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "lifecycle_msgs/msg/state.hpp" -#include "rclcpp/duration.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" - -namespace ros2_control -{ -/** - * @brief Class to handle asynchronous function calls. - * AsyncFunctionHandler is a class that allows the user to have a ascynchronous call to the parsed - * method, when the lifecycyle state is in the ACTIVE state - */ -template -class AsyncFunctionHandler -{ -public: - AsyncFunctionHandler() = default; - - ~AsyncFunctionHandler() { stop_async_update(); } - - /// Initialize the AsyncFunctionHandler with the get_state_function and async_function - /** - * @param get_state_function Function that returns the current lifecycle state - * @param async_function Function that will be called asynchronously - * If the AsyncFunctionHandler is already initialized and is running, it will throw a runtime - * error. - * If the parsed functions are not valid, it will throw a runtime error. - */ - void init( - std::function get_state_function, - std::function async_function) - { - if (get_state_function == nullptr) - { - throw std::runtime_error( - "AsyncFunctionHandler: parsed function to get the lifecycle state is not valid!"); - } - if (async_function == nullptr) - { - throw std::runtime_error( - "AsyncFunctionHandler: parsed function to call asynchronously is not valid!"); - } - if (thread_.joinable()) - { - throw std::runtime_error( - "AsyncFunctionHandler: Cannot reinitialize while the thread is " - "running. Please stop the async update first!"); - } - get_state_function_ = get_state_function; - async_function_ = async_function; - } - - /// Triggers the async update method cycle - /** - * @param time Current time - * @param period Current period - * @return A pair with the first element being a boolean indicating if the async update method was - * triggered and the second element being the last return value of the async function. - * If the AsyncFunctionHandler is not initialized properly, it will throw a runtime error. - * If the async update method is waiting for the trigger, it will notify the async thread to start - * the update cycle. - * If the async update method is still running, it will return the last return value of the async - * function. - * - * \note In the case of controllers, The controller manager is responsible - * for triggering and maintaining the controller's update rate, as it should be only acting as a - * scheduler. Same applies to the resource manager when handling the hardware components. - */ - std::pair trigger_async_update( - const rclcpp::Time & time, const rclcpp::Duration & period) - { - if (!is_initialized()) - { - throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); - } - if (!is_running()) - { - throw std::runtime_error( - "AsyncFunctionHandler: need to start the async update thread first before triggering!"); - } - std::unique_lock lock(async_mtx_, std::try_to_lock); - bool trigger_status = false; - if (lock.owns_lock() && !trigger_in_progress_) - { - { - std::unique_lock scoped_lock(std::move(lock)); - trigger_in_progress_ = true; - current_update_time_ = time; - current_update_period_ = period; - } - async_update_condition_.notify_one(); - trigger_status = true; - } - const T return_value = async_update_return_; - return std::make_pair(trigger_status, return_value); - } - - /// Waits until the current async update method cycle to finish - /** - * If the async method is running, it will wait for the current async method call to finish. - */ - void wait_for_trigger_cycle_to_finish() - { - if (is_running()) - { - std::unique_lock lock(async_mtx_); - cycle_end_condition_.wait(lock, [this] { return !trigger_in_progress_; }); - } - } - - /// Check if the AsyncFunctionHandler is initialized - /** - * @return True if the AsyncFunctionHandler is initialized, false otherwise - */ - bool is_initialized() const { return get_state_function_ && async_function_; } - - /// Join the async update thread - /** - * If the async method is running, it will join the async thread. - * If the async method is not running, it will return immediately. - */ - void join_async_update_thread() - { - if (is_running()) - { - thread_.join(); - } - } - - /// Check if the async update thread is running - /** - * @return True if the async update thread is running, false otherwise - */ - bool is_running() const { return thread_.joinable(); } - - /// Check if the async update is triggered to stop the cycle - /** - * @return True if the async update is stopped, false otherwise - */ - bool is_stopped() const - { - return ( - async_update_stop_ || - (is_initialized() && - get_state_function_().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)); - } - - /// Stops the async update thread - /** - * If the async method is running, it will notify the async thread to stop and then joins the - * async thread. - */ - void stop_async_update() - { - if (is_running()) - { - { - std::unique_lock lock(async_mtx_); - async_update_stop_ = true; - } - async_update_condition_.notify_one(); - thread_.join(); - } - } - - /// Initialize the async update thread - /** - * If the async update thread is not running, it will start the async update thread. - * If the async update thread is already configured and running, does nothing and return - * immediately. - */ - void start_async_update_thread() - { - if (!is_initialized()) - { - throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); - } - if (!thread_.joinable()) - { - async_update_stop_ = false; - trigger_in_progress_ = false; - async_update_return_ = T(); - thread_ = std::thread( - [this]() -> void - { - // \note There might be an concurrency issue with the get_state() call here. This mightn't - // be critical here as the state of the controller is not expected to change during the - // update cycle - while ((get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE || - get_state_function_().id() == - lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) && - !async_update_stop_) - { - { - std::unique_lock lock(async_mtx_); - async_update_condition_.wait( - lock, [this] { return trigger_in_progress_ || async_update_stop_; }); - if (!async_update_stop_) - { - async_update_return_ = - async_function_(current_update_time_, current_update_period_); - } - trigger_in_progress_ = false; - } - cycle_end_condition_.notify_all(); - } - }); - } - } - -private: - rclcpp::Time current_update_time_; - rclcpp::Duration current_update_period_{0, 0}; - - std::function get_state_function_; - std::function async_function_; - - // Async related variables - std::thread thread_; - std::atomic_bool async_update_stop_{false}; - std::atomic_bool trigger_in_progress_{false}; - std::atomic async_update_return_; - std::condition_variable async_update_condition_; - std::condition_variable cycle_end_condition_; - std::mutex async_mtx_; -}; -} // namespace ros2_control - -#endif // CONTROLLER_INTERFACE__ASYNC_FUNCTION_HANDLER_HPP_ diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 8d86dd7b29..bd38c75303 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -20,8 +20,8 @@ #include #include -#include "controller_interface/async_function_handler.hpp" #include "controller_interface/visibility_control.h" +#include "realtime_tools/async_function_handler.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/loaned_command_interface.hpp" @@ -305,7 +305,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; - std::unique_ptr> async_handler_; + std::unique_ptr> async_handler_; unsigned int update_rate_ = 0; bool is_async_ = false; std::string urdf_ = ""; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index e7719e053c..7e32f4db34 100644 --- a/controller_interface/package.xml +++ b/controller_interface/package.xml @@ -12,12 +12,16 @@ ament_cmake_gen_version_h hardware_interface + realtime_tools rclcpp_lifecycle sensor_msgs hardware_interface + realtime_tools rclcpp_lifecycle + realtime_tools + ament_cmake_gmock sensor_msgs diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 2741c67289..956560bacc 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -96,7 +96,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } if (is_async_) { - async_handler_ = std::make_unique>(); + async_handler_ = std::make_unique>(); async_handler_->init( std::bind(&ControllerInterfaceBase::get_state, this), std::bind( diff --git a/controller_interface/test/test_async_function_handler.cpp b/controller_interface/test/test_async_function_handler.cpp deleted file mode 100644 index 70cb96370d..0000000000 --- a/controller_interface/test/test_async_function_handler.cpp +++ /dev/null @@ -1,231 +0,0 @@ -// Copyright 2024 PAL Robotics S.L. -// -// 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. - -#include "test_async_function_handler.hpp" -#include "gmock/gmock.h" - -namespace controller_interface -{ -TestAsyncFunctionHandler::TestAsyncFunctionHandler() -: state_(rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "test_async")), - counter_(0), - return_state_(return_type::OK) -{ -} - -void TestAsyncFunctionHandler::initialize() -{ - handler_.init( - std::bind(&TestAsyncFunctionHandler::get_state, this), - std::bind( - &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2)); -} - -return_type TestAsyncFunctionHandler::trigger() -{ - return handler_.trigger_async_update(last_callback_time_, last_callback_period_).second; -} - -return_type TestAsyncFunctionHandler::update( - const rclcpp::Time & time, const rclcpp::Duration & period) -{ - last_callback_time_ = time; - last_callback_period_ = period; - // to simulate some work being done - std::this_thread::sleep_for(std::chrono::microseconds(10)); - counter_++; - return return_state_; -} -const rclcpp_lifecycle::State & TestAsyncFunctionHandler::get_state() const { return state_; } - -int TestAsyncFunctionHandler::get_counter() const { return counter_; } -void TestAsyncFunctionHandler::activate() -{ - state_ = - rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, state_.label()); -} - -void TestAsyncFunctionHandler::deactivate() -{ - state_ = - rclcpp_lifecycle::State(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, state_.label()); -} -} // namespace controller_interface -class AsyncFunctionHandlerTest : public ::testing::Test -{ -public: - static void SetUpTestCase() { rclcpp::init(0, nullptr); } - - static void TearDownTestCase() { rclcpp::shutdown(); } -}; - -TEST_F(AsyncFunctionHandlerTest, check_initialization) -{ - controller_interface::TestAsyncFunctionHandler async_class; - - ASSERT_FALSE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - - // It should not be possible to initialize setting wrong functions - ASSERT_THROW(async_class.get_handler().init(nullptr, nullptr), std::runtime_error); - - async_class.initialize(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - - // Once initialized, it should not be possible to initialize again - async_class.get_handler().start_async_update_thread(); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - ASSERT_THROW(async_class.initialize(), std::runtime_error); -} - -TEST_F(AsyncFunctionHandlerTest, check_triggering) -{ - controller_interface::TestAsyncFunctionHandler async_class; - - ASSERT_FALSE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - // It shouldn't be possible to trigger without initialization - ASSERT_THROW(async_class.trigger(), std::runtime_error); - - async_class.initialize(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - // It shouldn't be possible to trigger without starting the thread - ASSERT_THROW(async_class.trigger(), std::runtime_error); - async_class.get_handler().start_async_update_thread(); - - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); - ASSERT_EQ(async_class.get_counter(), 1); - - // Trigger one more cycle - // std::this_thread::sleep_for(std::chrono::microseconds(1)); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - std::this_thread::sleep_for(std::chrono::microseconds(1)); - async_class.get_handler().stop_async_update(); - ASSERT_EQ(async_class.get_counter(), 2); - - // now the async update should be preempted - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); -} - -TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) -{ - controller_interface::TestAsyncFunctionHandler async_class; - - async_class.initialize(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().start_async_update_thread(); - - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - for (int i = 1; i < 1e4; i++) - { - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); - ASSERT_EQ(async_class.get_counter(), i); - std::this_thread::sleep_for(std::chrono::microseconds(1)); - } - async_class.get_handler().stop_async_update(); - - // now the async update should be preempted - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); -} - -TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) -{ - controller_interface::TestAsyncFunctionHandler async_class; - - // Start with a deactivated state - async_class.initialize(); - async_class.deactivate(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - async_class.get_handler().start_async_update_thread(); - - // The thread will start and end immediately when invoked in inactive state - async_class.get_handler().join_async_update_thread(); - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - ASSERT_THROW(async_class.trigger(), std::runtime_error); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - - // Now activate it and launch again - async_class.activate(); - async_class.get_handler().start_async_update_thread(); - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - for (int i = 1; i < 100; i++) - { - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); - ASSERT_EQ(async_class.get_counter(), i); - std::this_thread::sleep_for(std::chrono::microseconds(1)); - } - - // Now let's do one more trigger cycle and then change the state to deactivate, then it should end - // the thread once the cycle is finished - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - async_class.deactivate(); - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - async_class.get_handler().join_async_update_thread(); - ASSERT_TRUE(async_class.get_handler().is_initialized()); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - - // Now let's test the case of activating it and then deactivating it when the thread is waiting - // for a trigger to start new update cycle execution - async_class.activate(); - async_class.get_handler().start_async_update_thread(); - - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(controller_interface::return_type::OK, async_class.trigger()); - async_class.get_handler().wait_for_trigger_cycle_to_finish(); - async_class.deactivate(); - EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); - // Now they continue to wait until a new cycle is triggered or the preempt is called - ASSERT_TRUE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); - - // now the async update should be preempted - async_class.get_handler().stop_async_update(); - ASSERT_FALSE(async_class.get_handler().is_running()); - ASSERT_TRUE(async_class.get_handler().is_stopped()); -} diff --git a/controller_interface/test/test_async_function_handler.hpp b/controller_interface/test/test_async_function_handler.hpp deleted file mode 100644 index 0a414786fa..0000000000 --- a/controller_interface/test/test_async_function_handler.hpp +++ /dev/null @@ -1,53 +0,0 @@ -// Copyright 2024 PAL Robotics S.L. -// -// 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 TEST_ASYNC_FUNCTION_HANDLER_HPP_ -#define TEST_ASYNC_FUNCTION_HANDLER_HPP_ - -#include "controller_interface/async_function_handler.hpp" -#include "controller_interface/controller_interface_base.hpp" - -namespace controller_interface -{ -class TestAsyncFunctionHandler -{ -public: - TestAsyncFunctionHandler(); - - void initialize(); - - ros2_control::AsyncFunctionHandler & get_handler() { return handler_; } - - return_type trigger(); - - return_type update(const rclcpp::Time & time, const rclcpp::Duration & period); - - const rclcpp_lifecycle::State & get_state() const; - - int get_counter() const; - - void activate(); - - void deactivate(); - -private: - rclcpp_lifecycle::State state_; - rclcpp::Time last_callback_time_; - rclcpp::Duration last_callback_period_{0, 0}; - int counter_; - return_type return_state_; - ros2_control::AsyncFunctionHandler handler_; -}; -} // namespace controller_interface -#endif // TEST_ASYNC_FUNCTION_HANDLER_HPP_ From 469682ad53a1d1effd8354a49f0e114cf5a45a05 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 12 Jun 2024 13:45:42 +0200 Subject: [PATCH 76/84] start the async thread when configuring the controller --- .../src/controller_interface_base.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 956560bacc..fa1558e5eb 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -57,15 +57,8 @@ return_type ControllerInterfaceBase::init( node_->register_on_cleanup( std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); - auto activate_callback = [this](const rclcpp_lifecycle::State & state) - { - if (is_async() && async_handler_) - { - async_handler_->start_async_update_thread(); - } - return on_activate(state); - }; - node_->register_on_activate(activate_callback); + node_->register_on_activate( + std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); node_->register_on_deactivate( std::bind(&ControllerInterfaceBase::on_deactivate, this, std::placeholders::_1)); @@ -101,6 +94,7 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() std::bind(&ControllerInterfaceBase::get_state, this), std::bind( &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); + async_handler_->start_async_update_thread(); } return get_node()->configure(); From f3b5d436fb9af22b626278cfdaf954954782645c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Jun 2024 14:40:00 +0200 Subject: [PATCH 77/84] remove the get_state bind for API change --- controller_interface/src/controller_interface_base.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index fa1558e5eb..a3fec674fe 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -91,7 +91,6 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() { async_handler_ = std::make_unique>(); async_handler_->init( - std::bind(&ControllerInterfaceBase::get_state, this), std::bind( &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); async_handler_->start_async_update_thread(); From 408780c1c78390eb6c87d2bb14b8a7810bef1c1f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jul 2024 16:33:08 +0200 Subject: [PATCH 78/84] add new API naming changes --- .../src/controller_interface_base.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index a3fec674fe..b373edc98b 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -90,10 +90,9 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() if (is_async_) { async_handler_ = std::make_unique>(); - async_handler_->init( - std::bind( - &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); - async_handler_->start_async_update_thread(); + async_handler_->init(std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); + async_handler_->start_thread(); } return get_node()->configure(); @@ -123,7 +122,7 @@ std::pair ControllerInterfaceBase::trigger_update( { if (is_async()) { - return async_handler_->trigger_async_update(time, period); + return async_handler_->trigger_async_callback(time, period); } else { @@ -159,7 +158,7 @@ void ControllerInterfaceBase::stop_async_update_cycle() { if (is_async() && async_handler_ && async_handler_->is_running()) { - async_handler_->stop_async_update(); + async_handler_->stop_thread(); } } } // namespace controller_interface From 8f10adc41bb1ec4464d86ba72be9afff3f2f7f2f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 5 Aug 2024 18:20:03 +0200 Subject: [PATCH 79/84] added thread priority argument to be able to set the scheduler priority --- .../src/controller_interface_base.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index b373edc98b..4a7ad3e6de 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -35,6 +35,7 @@ return_type ControllerInterfaceBase::init( { auto_declare("update_rate", cm_update_rate); auto_declare("is_async", false); + auto_declare("thread_priority", 50); } catch (const std::exception & e) { @@ -89,9 +90,16 @@ const rclcpp_lifecycle::State & ControllerInterfaceBase::configure() } if (is_async_) { + const unsigned int thread_priority = + static_cast(get_node()->get_parameter("thread_priority").as_int()); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "Starting async handler with scheduler priority: " << thread_priority); async_handler_ = std::make_unique>(); - async_handler_->init(std::bind( - &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2)); + async_handler_->init( + std::bind( + &ControllerInterfaceBase::update, this, std::placeholders::_1, std::placeholders::_2), + thread_priority); async_handler_->start_thread(); } From a40b0050335e9fd3135ddeda8c7c9f1de907a350 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Sep 2024 19:47:44 +0200 Subject: [PATCH 80/84] stop the thread on cleanup of the controller --- controller_interface/src/controller_interface_base.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 4a7ad3e6de..4c46913ce0 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -56,7 +56,14 @@ return_type ControllerInterfaceBase::init( std::bind(&ControllerInterfaceBase::on_configure, this, std::placeholders::_1)); node_->register_on_cleanup( - std::bind(&ControllerInterfaceBase::on_cleanup, this, std::placeholders::_1)); + [this](const rclcpp_lifecycle::State & previous_state) -> CallbackReturn + { + if (is_async() && async_handler_ && async_handler_->is_running()) + { + async_handler_->stop_thread(); + } + return on_cleanup(previous_state); + }); node_->register_on_activate( std::bind(&ControllerInterfaceBase::on_activate, this, std::placeholders::_1)); From aa9a81e268878b8ea6f1228e5df5489d205f78d1 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Sep 2024 19:48:21 +0200 Subject: [PATCH 81/84] wait for cycle to finish in the switch controllers before deactivating the controller --- .../controller_interface_base.hpp | 15 +++++++-------- .../src/controller_interface_base.cpp | 4 ++-- controller_manager/src/controller_manager.cpp | 14 ++++++++++++-- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index bd38c75303..1a10067c01 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -286,18 +286,17 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy virtual bool is_in_chained_mode() const = 0; /** - * Method to stop any running async update cycle after finishing the current cycle. This is needed - * to be called before deactivating the controller by the controller_manager, so that the - * interfaces still exist when the controller finishes its cycle and then it's exits. + * Method to wait for any running async update cycle to finish after finishing the current cycle. + * This is needed to be called before deactivating the controller by the controller_manager, so + * that the interfaces still exist when the controller finishes its cycle and then it's exits. * - * **The method is not real-time safe and shouldn't be called in the control loop.** + * \note **The method is not real-time safe and shouldn't be called in the control loop.** * - * If the controller is running in async mode, the method will stop after the current update cycle - * execution. - * If the controller is not running in async mode, the method will do nothing. + * If the controller is running in async mode, the method will wait for the current async update + * to finish. If the controller is not running in async mode, the method will do nothing. */ CONTROLLER_INTERFACE_PUBLIC - void stop_async_update_cycle(); + void wait_for_trigger_update_to_finish(); protected: std::vector command_interfaces_; diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 4c46913ce0..3de8ada109 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -169,11 +169,11 @@ bool ControllerInterfaceBase::is_async() const { return is_async_; } const std::string & ControllerInterfaceBase::get_robot_description() const { return urdf_; } -void ControllerInterfaceBase::stop_async_update_cycle() +void ControllerInterfaceBase::wait_for_trigger_update_to_finish() { if (is_async() && async_handler_ && async_handler_->is_running()) { - async_handler_->stop_thread(); + async_handler_->wait_for_trigger_cycle_to_finish(); } } } // namespace controller_interface diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 15774168f3..f36171c05d 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1296,6 +1296,18 @@ controller_interface::return_type ControllerManager::switch_controller( RCLCPP_DEBUG(get_logger(), " - %s", interface.c_str()); } + // wait for deactivating async controllers to finish their current cycle + for (const auto & controller : deactivate_request_) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller)); + if (controller_it != controllers.end()) + { + controller_it->c->wait_for_trigger_update_to_finish(); + } + } + if ( !activate_command_interface_request_.empty() || !deactivate_command_interface_request_.empty()) { @@ -1475,8 +1487,6 @@ void ControllerManager::deactivate_controllers( { try { - // This is needed by the async controllers to finish their current cycle - controller->stop_async_update_cycle(); const auto new_state = controller->get_node()->deactivate(); controller->release_interfaces(); From 023a2041197758a4e0076e9e458ea54ed16beca5 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Sep 2024 20:31:28 +0200 Subject: [PATCH 82/84] fix the thread_priority parameter declaration type --- controller_interface/src/controller_interface_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_interface/src/controller_interface_base.cpp b/controller_interface/src/controller_interface_base.cpp index 3de8ada109..6148dc7d63 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -35,7 +35,7 @@ return_type ControllerInterfaceBase::init( { auto_declare("update_rate", cm_update_rate); auto_declare("is_async", false); - auto_declare("thread_priority", 50); + auto_declare("thread_priority", 50); } catch (const std::exception & e) { From 3bd19bf7c83d3551ace29c0a7e84206aedfae629 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Sep 2024 20:37:50 +0200 Subject: [PATCH 83/84] change conditioning to not trigger logging for a failed return as well --- controller_manager/src/controller_manager.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f36171c05d..b0600d59aa 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -2310,7 +2310,13 @@ controller_interface::return_type ControllerManager::update( } *loaded_controller.next_update_cycle_time += controller_period; - if (!trigger_status) + + if (controller_ret != controller_interface::return_type::OK) + { + failed_controllers_list.push_back(loaded_controller.info.name); + ret = controller_ret; + } + else if (!trigger_status) { RCLCPP_WARN( get_logger(), @@ -2319,12 +2325,6 @@ controller_interface::return_type ControllerManager::update( loaded_controller.info.name.c_str(), time.seconds(), loaded_controller.next_update_cycle_time->seconds()); } - - if (controller_ret != controller_interface::return_type::OK) - { - failed_controllers_list.push_back(loaded_controller.info.name); - ret = controller_ret; - } } } } From ebb69c58a7121f66c6f88ca9b536e62a21fd0ee9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 13 Sep 2024 20:38:15 +0200 Subject: [PATCH 84/84] Add tests for the async controller --- .../test/test_controller/test_controller.cpp | 4 + .../test/test_controller_manager.cpp | 185 ++++++++++++++++++ 2 files changed, 189 insertions(+) diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index 7b9af6ecfc..adf7e0e64b 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con controller_interface::return_type TestController::update( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { + if (is_async()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate()))); + } update_period_ = period; ++internal_counter; diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index 28e9f0477c..ca0f2838be 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -232,6 +232,191 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ(1, test_controller.use_count()); } +TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle) +{ + const auto test_param = GetParam(); + auto test_controller = std::make_shared(); + auto test_controller2 = std::make_shared(); + constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name"; + cm_->add_controller( + test_controller, test_controller::TEST_CONTROLLER_NAME, + test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ(2u, cm_->get_loaded_controllers().size()); + EXPECT_EQ(2, test_controller.use_count()); + + // setup interface to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller->set_command_interface_configuration(cmd_itfs_cfg); + + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller->set_state_interface_configuration(state_itfs_cfg); + + controller_interface::InterfaceConfiguration cmd_itfs_cfg2; + cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg2.names.push_back(interface); + } + test_controller2->set_command_interface_configuration(cmd_itfs_cfg2); + + controller_interface::InterfaceConfiguration state_itfs_cfg2; + state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL; + test_controller2->set_state_interface_configuration(state_itfs_cfg2); + + // Check if namespace is set correctly + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'", + cm_->get_namespace()); + EXPECT_STREQ(cm_->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'", + test_controller->get_node()->get_namespace()); + EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/"); + RCLCPP_INFO( + rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'", + test_controller2->get_node()->get_namespace()); + EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/"); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) + << "Update should not reach an unconfigured controller"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + + // configure controller + rclcpp::Parameter update_rate_parameter("update_rate", static_cast(20)); + rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true)); + test_controller->get_node()->set_parameter(update_rate_parameter); + test_controller->get_node()->set_parameter(is_async_parameter); + { + ControllerManagerRunner cm_runner(this); + cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); + cm_->configure_controller(TEST_CONTROLLER2_NAME); + } + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started"; + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + + // Start controller, will take effect at the end of the update function + std::vector start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME}; + std::vector stop_controllers = {}; + auto switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(test_param.expected_return, switch_future.get()); + } + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter); + + // Start the real test controller, will take effect at the end of the update function + start_controllers = {test_controller::TEST_CONTROLLER_NAME}; + stop_controllers = {}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id()); + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(test_controller->internal_counter, 0u); + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(test_controller->internal_counter, 1u); + size_t last_internal_counter = test_controller->internal_counter; + + // Stop controller, will take effect at the end of the update function + start_controllers = {}; + stop_controllers = {test_controller::TEST_CONTROLLER_NAME}; + switch_future = std::async( + std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, + start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0)); + + ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + << "switch_controller should be blocking until next update cycle"; + + EXPECT_EQ( + controller_interface::return_type::OK, + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); + EXPECT_EQ(last_internal_counter, test_controller->internal_counter) + << "This shouldn't have updated as this is async and in the controller it is waiting before " + "updating the counter"; + std::this_thread::sleep_for( + std::chrono::milliseconds(1000 / (test_controller->get_update_rate()))); + EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter) + << "Controller is stopped at the end of update, so it should have done one more update"; + { + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); + } + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller->get_lifecycle_state().id()); + auto unload_future = std::async( + std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_, + test_controller::TEST_CONTROLLER_NAME); + + ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100))) + << "unload_controller should be blocking until next update cycle"; + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(controller_interface::return_type::OK, unload_future.get()); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, + test_controller->get_lifecycle_state().id()); + EXPECT_EQ(1, test_controller.use_count()); +} + TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { auto strictness = GetParam().strictness;