From c37695338d5efab18332255f7d0e7d4e5db1420a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Jun 2024 14:51:09 +0200 Subject: [PATCH 01/13] moved the version of the async function handler from controller_interface --- CMakeLists.txt | 7 + .../realtime_tools/async_function_handler.hpp | 259 ++++++++++++++++++ package.xml | 2 + test/test_async_function_handler.cpp | 232 ++++++++++++++++ test/test_async_function_handler.hpp | 59 ++++ 5 files changed, 559 insertions(+) create mode 100644 include/realtime_tools/async_function_handler.hpp create mode 100644 test/test_async_function_handler.cpp create mode 100644 test/test_async_function_handler.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 21f2e96e..8cd349f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,6 +10,8 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_action Threads rcpputils + lifecycle_msgs + rclcpp_lifecycle ) find_package(ament_cmake REQUIRED) @@ -72,6 +74,11 @@ if(BUILD_TESTING) test/realtime_server_goal_handle_tests.cpp) target_link_libraries(realtime_server_goal_handle_tests realtime_tools) ament_target_dependencies(realtime_server_goal_handle_tests test_msgs) + + ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) + target_link_libraries(test_async_function_handler + realtime_tools + ) endif() # Install diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp new file mode 100644 index 00000000..a8864cab --- /dev/null +++ b/include/realtime_tools/async_function_handler.hpp @@ -0,0 +1,259 @@ +// 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 REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_ +#define REALTIME_TOOLS__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 realtime_tools +{ +/** + * @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 // REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_ diff --git a/package.xml b/package.xml index 7658c857..83aff1d7 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,8 @@ ament_cmake rclcpp + lifecycle_msgs + rclcpp_lifecycle rclcpp_action ament_cmake_gmock diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp new file mode 100644 index 00000000..0a9703f7 --- /dev/null +++ b/test/test_async_function_handler.cpp @@ -0,0 +1,232 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "gmock/gmock.h" + +namespace realtime_tools +{ +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 realtime_tools +class AsyncFunctionHandlerTest : public ::testing::Test +{ +public: + static void SetUpTestCase() { rclcpp::init(0, nullptr); } + + static void TearDownTestCase() { rclcpp::shutdown(); } +}; + +TEST_F(AsyncFunctionHandlerTest, check_initialization) +{ + realtime_tools::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(realtime_tools::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) +{ + realtime_tools::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(realtime_tools::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(realtime_tools::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) +{ + realtime_tools::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(realtime_tools::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) +{ + realtime_tools::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(realtime_tools::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(realtime_tools::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(realtime_tools::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/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp new file mode 100644 index 00000000..25eec475 --- /dev/null +++ b/test/test_async_function_handler.hpp @@ -0,0 +1,59 @@ +// 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 "realtime_tools/async_function_handler.hpp" + +namespace realtime_tools +{ +enum class return_type : std::uint8_t +{ + OK = 0, + ERROR = 1, + DEACTIVATE = 2, +}; + +class TestAsyncFunctionHandler +{ +public: + TestAsyncFunctionHandler(); + + void initialize(); + + realtime_tools::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_; + realtime_tools::AsyncFunctionHandler handler_; +}; +} // namespace realtime_tools +#endif // TEST_ASYNC_FUNCTION_HANDLER_HPP_ From 8d9d69773def9b709740694db42631ed48618e4c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Jun 2024 15:51:26 +0200 Subject: [PATCH 02/13] add pre-comitting changes of the realtime_tools repository --- .../realtime_tools/async_function_handler.hpp | 74 ++++++++----------- test/test_async_function_handler.cpp | 2 +- test/test_async_function_handler.hpp | 5 +- 3 files changed, 33 insertions(+), 48 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index a8864cab..134d5bcb 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -60,18 +60,15 @@ class AsyncFunctionHandler std::function get_state_function, std::function async_function) { - if (get_state_function == nullptr) - { + if (get_state_function == nullptr) { throw std::runtime_error( "AsyncFunctionHandler: parsed function to get the lifecycle state is not valid!"); } - if (async_function == nullptr) - { + if (async_function == nullptr) { throw std::runtime_error( "AsyncFunctionHandler: parsed function to call asynchronously is not valid!"); } - if (thread_.joinable()) - { + if (thread_.joinable()) { throw std::runtime_error( "AsyncFunctionHandler: Cannot reinitialize while the thread is " "running. Please stop the async update first!"); @@ -99,19 +96,16 @@ class AsyncFunctionHandler std::pair trigger_async_update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (!is_initialized()) - { + if (!is_initialized()) { throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } - if (!is_running()) - { + 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_) - { + if (lock.owns_lock() && !trigger_in_progress_) { { std::unique_lock scoped_lock(std::move(lock)); trigger_in_progress_ = true; @@ -131,8 +125,7 @@ class AsyncFunctionHandler */ void wait_for_trigger_cycle_to_finish() { - if (is_running()) - { + if (is_running()) { std::unique_lock lock(async_mtx_); cycle_end_condition_.wait(lock, [this] { return !trigger_in_progress_; }); } @@ -151,8 +144,7 @@ class AsyncFunctionHandler */ void join_async_update_thread() { - if (is_running()) - { + if (is_running()) { thread_.join(); } } @@ -182,8 +174,7 @@ class AsyncFunctionHandler */ void stop_async_update() { - if (is_running()) - { + if (is_running()) { { std::unique_lock lock(async_mtx_); async_update_stop_ = true; @@ -201,40 +192,33 @@ class AsyncFunctionHandler */ void start_async_update_thread() { - if (!is_initialized()) - { + if (!is_initialized()) { throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } - if (!thread_.joinable()) - { + 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_) + 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; + 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_); } - cycle_end_condition_.notify_all(); + trigger_in_progress_ = false; } - }); + cycle_end_condition_.notify_all(); + } + }); } } @@ -254,6 +238,6 @@ class AsyncFunctionHandler std::condition_variable cycle_end_condition_; std::mutex async_mtx_; }; -} // namespace ros2_control +} // namespace realtime_tools #endif // REALTIME_TOOLS__ASYNC_FUNCTION_HANDLER_HPP_ diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 0a9703f7..32e56cdc 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -13,8 +13,8 @@ // limitations under the License. #include "test_async_function_handler.hpp" -#include "rclcpp/rclcpp.hpp" #include "gmock/gmock.h" +#include "rclcpp/rclcpp.hpp" namespace realtime_tools { diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index 25eec475..39dc69fb 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -15,12 +15,13 @@ #ifndef TEST_ASYNC_FUNCTION_HANDLER_HPP_ #define TEST_ASYNC_FUNCTION_HANDLER_HPP_ +#include + #include "realtime_tools/async_function_handler.hpp" namespace realtime_tools { -enum class return_type : std::uint8_t -{ +enum class return_type : std::uint8_t { OK = 0, ERROR = 1, DEACTIVATE = 2, From 3edbb68bb0663a36c89b0a86a2f6ce1103cf3c60 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Jun 2024 15:56:10 +0200 Subject: [PATCH 03/13] use the complete pair for testing and also check for missing triggers by removing 1us sleep --- test/test_async_function_handler.cpp | 62 ++++++++++++++++++---------- test/test_async_function_handler.hpp | 2 +- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 32e56cdc..b73cfaac 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -33,9 +33,9 @@ void TestAsyncFunctionHandler::initialize() &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2)); } -return_type TestAsyncFunctionHandler::trigger() +std::pair TestAsyncFunctionHandler::trigger() { - return handler_.trigger_async_update(last_callback_time_, last_callback_period_).second; + return handler_.trigger_async_update(last_callback_time_, last_callback_period_); } return_type TestAsyncFunctionHandler::update( @@ -89,7 +89,9 @@ TEST_F(AsyncFunctionHandlerTest, check_initialization) // Once initialized, it should not be possible to initialize again async_class.get_handler().start_async_update_thread(); - ASSERT_EQ(realtime_tools::return_type::OK, async_class.trigger()); + auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); @@ -115,7 +117,9 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(realtime_tools::return_type::OK, async_class.trigger()); + auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); @@ -123,14 +127,14 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_EQ(async_class.get_counter(), 1); // Trigger one more cycle - // std::this_thread::sleep_for(std::chrono::microseconds(1)); - ASSERT_EQ(realtime_tools::return_type::OK, async_class.trigger()); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); 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); + ASSERT_LE(async_class.get_counter(), 2); // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); @@ -149,16 +153,25 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) 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(realtime_tools::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)); + + int missed_triggers = 0; + const int total_cycles = 1e5; + for (int i = 1; i < total_cycles; i++) { + const auto trigger_status = async_class.trigger(); + if (trigger_status.first) { + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + 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 - missed_triggers); + } else { + missed_triggers++; + } } + // Make sure that the failed triggers are less than 0.1% + ASSERT_LT(missed_triggers, static_cast(0.001 * total_cycles)) + << "The missed triggers cannot be more than 0.1%!"; async_class.get_handler().stop_async_update(); // now the async update should be preempted @@ -190,9 +203,10 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) 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(realtime_tools::return_type::OK, async_class.trigger()); + for (int i = 1; i < 100; i++) { + const auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); @@ -203,7 +217,9 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) // 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(realtime_tools::return_type::OK, async_class.trigger()); + auto trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); 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(); @@ -217,7 +233,9 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) async_class.get_handler().start_async_update_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); - ASSERT_EQ(realtime_tools::return_type::OK, async_class.trigger()); + trigger_status = async_class.trigger(); + ASSERT_TRUE(trigger_status.first); + ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); 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); diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index 39dc69fb..f1373369 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -36,7 +36,7 @@ class TestAsyncFunctionHandler realtime_tools::AsyncFunctionHandler & get_handler() { return handler_; } - return_type trigger(); + std::pair trigger(); return_type update(const rclcpp::Time & time, const rclcpp::Duration & period); From bdc0ce06c6a3dd6a1ae74d5e0b1988bbffcc7740 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 10 Jun 2024 16:35:00 +0200 Subject: [PATCH 04/13] Add thread priority to the API to configure the scheduler --- CMakeLists.txt | 1 + .../realtime_tools/async_function_handler.hpp | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8cd349f4..622fa9b9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,7 @@ if(BUILD_TESTING) ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) target_link_libraries(test_async_function_handler realtime_tools + thread_priority ) endif() diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 134d5bcb..a467b5a0 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -20,6 +20,7 @@ #include #include #include +#include #include #include #include @@ -30,8 +31,10 @@ #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" +#include "rclcpp/logging.hpp" #include "rclcpp/time.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "realtime_tools/thread_priority.hpp" namespace realtime_tools { @@ -58,7 +61,8 @@ class AsyncFunctionHandler */ void init( std::function get_state_function, - std::function async_function) + std::function async_function, + int thread_priority = 50) { if (get_state_function == nullptr) { throw std::runtime_error( @@ -75,6 +79,7 @@ class AsyncFunctionHandler } get_state_function_ = get_state_function; async_function_ = async_function; + thread_priority_ = thread_priority; } /// Triggers the async update method cycle @@ -200,6 +205,15 @@ class AsyncFunctionHandler trigger_in_progress_ = false; async_update_return_ = T(); thread_ = std::thread([this]() -> void { + if (!realtime_tools::configure_sched_fifo(thread_priority_)) { + RCLCPP_WARN( + rclcpp::get_logger("AsyncFunctionHandler"), + "Could not enable FIFO RT scheduling policy. Consider setting up your user to do FIFO " + "RT " + "scheduling. See " + "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " + "for details."); + } // \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 @@ -231,6 +245,7 @@ class AsyncFunctionHandler // Async related variables std::thread thread_; + int thread_priority_ = std::numeric_limits::quiet_NaN(); std::atomic_bool async_update_stop_{false}; std::atomic_bool trigger_in_progress_{false}; std::atomic async_update_return_; From 8479170b8d46d245e2811d21586ee20c6164972d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 11 Jun 2024 19:31:53 +0200 Subject: [PATCH 05/13] change the logic wrt to the lifecycle state --- .../realtime_tools/async_function_handler.hpp | 17 +++++----------- test/test_async_function_handler.cpp | 20 +++++++------------ 2 files changed, 12 insertions(+), 25 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index a467b5a0..dc355407 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -164,13 +164,7 @@ class AsyncFunctionHandler /** * @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)); - } + bool is_stopped() const { return async_update_stop_; } /// Stops the async update thread /** @@ -217,15 +211,14 @@ 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 || - get_state_function_().id() == lifecycle_msgs::msg::State::TRANSITION_STATE_ACTIVATING) && - !async_update_stop_) { + while (!async_update_stop_.load(std::memory_order_relaxed)) { { 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_ && + (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) { async_update_return_ = async_function_(current_update_time_, current_update_period_); } trigger_in_progress_ = false; diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index b73cfaac..7b5ef930 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -186,18 +186,13 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) // Start with a deactivated state async_class.initialize(); async_class.deactivate(); + ASSERT_THROW(async_class.trigger(), std::runtime_error) + << "Should throw before starting a 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()); + ASSERT_FALSE(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(); @@ -221,16 +216,15 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_TRUE(trigger_status.first); ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); async_class.deactivate(); + async_class.get_handler().wait_for_trigger_cycle_to_finish(); 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()); + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(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); trigger_status = async_class.trigger(); @@ -241,7 +235,7 @@ 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_stopped()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); // now the async update should be preempted async_class.get_handler().stop_async_update(); From 2e6d7c3a36597b9f644ca423711562dc1ddf8835 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 11 Jun 2024 20:04:43 +0200 Subject: [PATCH 06/13] change the triggering to the ACTIVE lifecycle state only --- include/realtime_tools/async_function_handler.hpp | 8 ++++---- test/test_async_function_handler.cpp | 12 ++++++++++-- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index dc355407..406b1e40 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -110,7 +110,9 @@ class AsyncFunctionHandler } std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; - if (lock.owns_lock() && !trigger_in_progress_) { + if ( + lock.owns_lock() && !trigger_in_progress_ && + get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { { std::unique_lock scoped_lock(std::move(lock)); trigger_in_progress_ = true; @@ -216,9 +218,7 @@ 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_ && - (get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)) { + if (!async_update_stop_) { async_update_return_ = async_function_(current_update_time_, current_update_period_); } trigger_in_progress_ = false; diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 7b5ef930..069c2221 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -192,13 +192,15 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); async_class.get_handler().start_async_update_thread(); + ASSERT_TRUE(async_class.get_handler().is_running()); + ASSERT_FALSE(async_class.get_handler().is_stopped()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); // 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++) { + const int total_cycles = 100; + for (int i = 1; i < total_cycles; i++) { const auto trigger_status = async_class.trigger(); ASSERT_TRUE(trigger_status.first); ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); @@ -217,6 +219,12 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); async_class.deactivate(); async_class.get_handler().wait_for_trigger_cycle_to_finish(); + for (int i = 0; i < 50; i++) { + const auto trigger_status = async_class.trigger(); + ASSERT_FALSE(trigger_status.first); + ASSERT_EQ(async_class.get_counter(), total_cycles) + << "The trigger should fail for any state different than ACTIVE"; + } EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); ASSERT_TRUE(async_class.get_handler().is_initialized()); ASSERT_TRUE(async_class.get_handler().is_running()); From ecf4610fc21d39cc769ee96c539437fbffaacc65 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 12 Jun 2024 19:27:40 +0200 Subject: [PATCH 07/13] Add trigger predicate to check before triggering --- .../realtime_tools/async_function_handler.hpp | 25 ++++++++++++++++--- test/test_async_function_handler.cpp | 3 ++- 2 files changed, 24 insertions(+), 4 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 406b1e40..9bd077e9 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -82,6 +82,26 @@ class AsyncFunctionHandler thread_priority_ = thread_priority; } + /// Initialize the AsyncFunctionHandler with the get_state_function, async_function and + /// trigger_predicate + /** + * @param get_state_function Function that returns the current lifecycle state + * @param async_function Function that will be called asynchronously + * @param trigger_predicate Predicate function that will be called to check if the async update + * method should be triggered + * 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, + std::function trigger_predicate, int thread_priority = 50) + { + init(get_state_function, async_function, thread_priority); + trigger_predicate_ = trigger_predicate; + } + /// Triggers the async update method cycle /** * @param time Current time @@ -110,9 +130,7 @@ class AsyncFunctionHandler } std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; - if ( - lock.owns_lock() && !trigger_in_progress_ && - get_state_function_().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + if (lock.owns_lock() && !trigger_in_progress_ && trigger_predicate_()) { { std::unique_lock scoped_lock(std::move(lock)); trigger_in_progress_ = true; @@ -235,6 +253,7 @@ class AsyncFunctionHandler std::function get_state_function_; std::function async_function_; + std::function trigger_predicate_ = []() { return true; }; // Async related variables std::thread thread_; diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 069c2221..bbe274d6 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -30,7 +30,8 @@ void TestAsyncFunctionHandler::initialize() handler_.init( std::bind(&TestAsyncFunctionHandler::get_state, this), std::bind( - &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2)); + &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2), + [this]() { return state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; }); } std::pair TestAsyncFunctionHandler::trigger() From 9695ad63850212a25bb707c04a9e868d620a861f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Jun 2024 13:58:54 +0200 Subject: [PATCH 08/13] remove get_state function and focus on the trigger_predicate_ which does the same --- CMakeLists.txt | 10 ++--- .../realtime_tools/async_function_handler.hpp | 38 ++++++++----------- package.xml | 4 +- test/test_async_function_handler.cpp | 1 - test/test_async_function_handler.hpp | 2 + 5 files changed, 23 insertions(+), 32 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 622fa9b9..ad4e961e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,8 +10,6 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS rclcpp_action Threads rcpputils - lifecycle_msgs - rclcpp_lifecycle ) find_package(ament_cmake REQUIRED) @@ -43,6 +41,8 @@ ament_target_dependencies(thread_priority PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} # Unit Tests if(BUILD_TESTING) find_package(ament_cmake_gmock REQUIRED) + find_package(lifecycle_msgs REQUIRED) + find_package(rclcpp_lifecycle REQUIRED) find_package(test_msgs REQUIRED) ament_add_gmock(realtime_box_tests test/realtime_box_tests.cpp) @@ -76,10 +76,8 @@ if(BUILD_TESTING) ament_target_dependencies(realtime_server_goal_handle_tests test_msgs) ament_add_gmock(test_async_function_handler test/test_async_function_handler.cpp) - target_link_libraries(test_async_function_handler - realtime_tools - thread_priority - ) + target_link_libraries(test_async_function_handler realtime_tools thread_priority) + ament_target_dependencies(test_async_function_handler lifecycle_msgs rclcpp_lifecycle) endif() # Install diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 9bd077e9..cf963ca5 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -29,19 +29,17 @@ #include #include -#include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/time.hpp" -#include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/thread_priority.hpp" namespace realtime_tools { /** * @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 + * AsyncFunctionHandler is a class that allows the user to have a asynchronous call to the parsed + * method and be able to set some thread specific parameters */ template class AsyncFunctionHandler @@ -51,23 +49,17 @@ class AsyncFunctionHandler ~AsyncFunctionHandler() { stop_async_update(); } - /// Initialize the AsyncFunctionHandler with the get_state_function and async_function + /// Initialize the AsyncFunctionHandler with the async_function and thread_priority /** - * @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, int thread_priority = 50) { - 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!"); @@ -77,28 +69,31 @@ class AsyncFunctionHandler "AsyncFunctionHandler: Cannot reinitialize while the thread is " "running. Please stop the async update first!"); } - get_state_function_ = get_state_function; async_function_ = async_function; thread_priority_ = thread_priority; } - /// Initialize the AsyncFunctionHandler with the get_state_function, async_function and - /// trigger_predicate + /// Initialize the AsyncFunctionHandler with the async_function and trigger_predicate /** - * @param get_state_function Function that returns the current lifecycle state * @param async_function Function that will be called asynchronously * @param trigger_predicate Predicate function that will be called to check if the async update - * method should be triggered + * method should be triggered. + * + * \note The parsed trigger_predicate should be free from any concurrency issues. It is expected + * to be both thread-safe and reentrant. + * * 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, std::function trigger_predicate, int thread_priority = 50) { - init(get_state_function, async_function, thread_priority); + if (trigger_predicate == nullptr) { + throw std::runtime_error("AsyncFunctionHandler: parsed trigger predicate is not valid!"); + } + init(async_function, thread_priority); trigger_predicate_ = trigger_predicate; } @@ -160,7 +155,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 async_function_ && trigger_predicate_; } /// Join the async update thread /** @@ -228,9 +223,7 @@ class AsyncFunctionHandler "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " "for details."); } - // \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 (!async_update_stop_.load(std::memory_order_relaxed)) { { std::unique_lock lock(async_mtx_); @@ -251,7 +244,6 @@ class AsyncFunctionHandler rclcpp::Time current_update_time_; rclcpp::Duration current_update_period_{0, 0}; - std::function get_state_function_; std::function async_function_; std::function trigger_predicate_ = []() { return true; }; diff --git a/package.xml b/package.xml index 83aff1d7..8f575d2c 100644 --- a/package.xml +++ b/package.xml @@ -20,11 +20,11 @@ ament_cmake rclcpp - lifecycle_msgs - rclcpp_lifecycle rclcpp_action ament_cmake_gmock + lifecycle_msgs + rclcpp_lifecycle test_msgs diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index bbe274d6..84b1de5c 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -28,7 +28,6 @@ TestAsyncFunctionHandler::TestAsyncFunctionHandler() void TestAsyncFunctionHandler::initialize() { handler_.init( - std::bind(&TestAsyncFunctionHandler::get_state, this), std::bind( &TestAsyncFunctionHandler::update, this, std::placeholders::_1, std::placeholders::_2), [this]() { return state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; }); diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index f1373369..e66af6bd 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -17,6 +17,8 @@ #include +#include "lifecycle_msgs/msg/state.hpp" +#include "rclcpp_lifecycle/state.hpp" #include "realtime_tools/async_function_handler.hpp" namespace realtime_tools From 28bb919205ecfc254315c20d9ee19ebb01bd602d Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 13 Jun 2024 14:39:29 +0200 Subject: [PATCH 09/13] added new API to get the threads and also the progress boolean --- include/realtime_tools/async_function_handler.hpp | 12 ++++++++++++ test/test_async_function_handler.cpp | 6 ++++++ 2 files changed, 18 insertions(+) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index cf963ca5..6137508e 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -181,6 +181,18 @@ class AsyncFunctionHandler */ bool is_stopped() const { return async_update_stop_; } + /// Get the async update thread + /** + * @return The async update thread + */ + std::thread & get_thread() { return thread_; } + + /// Check if the async update method is in progress + /** + * @return True if the async update method is in progress, false otherwise + */ + bool is_trigger_cycle_in_progress() const { return trigger_in_progress_; } + /// Stops the async update thread /** * If the async method is running, it will notify the async thread to stop and then joins the diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 84b1de5c..7335a818 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -123,7 +123,10 @@ 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()); + ASSERT_TRUE(async_class.get_handler().get_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); + ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); ASSERT_EQ(async_class.get_counter(), 1); // Trigger one more cycle @@ -164,6 +167,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) 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_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); ASSERT_EQ(async_class.get_counter(), i - missed_triggers); } else { missed_triggers++; @@ -204,10 +208,12 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) const auto trigger_status = async_class.trigger(); ASSERT_TRUE(trigger_status.first); ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); + ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); 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_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); ASSERT_EQ(async_class.get_counter(), i); std::this_thread::sleep_for(std::chrono::microseconds(1)); } From 2c12ab84c6b9584f4c1306264e6153924239b5a6 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 16 Jun 2024 18:24:41 +0200 Subject: [PATCH 10/13] change the method name to get_async_thread --- include/realtime_tools/async_function_handler.hpp | 2 +- test/test_async_function_handler.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 6137508e..7f5c67db 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -185,7 +185,7 @@ class AsyncFunctionHandler /** * @return The async update thread */ - std::thread & get_thread() { return thread_; } + std::thread & get_async_thread() { return thread_; } /// Check if the async update method is in progress /** diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 7335a818..0139a7d8 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -123,7 +123,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()); - ASSERT_TRUE(async_class.get_handler().get_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().get_async_thread().joinable()); ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); From abc9d1245e20772fca4568a589f8fff440dcaffa Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 17 Jun 2024 21:14:55 +0200 Subject: [PATCH 11/13] addeed thread_priority to the documentation --- include/realtime_tools/async_function_handler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index 7f5c67db..f4ed63ea 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -78,6 +78,7 @@ class AsyncFunctionHandler * @param async_function Function that will be called asynchronously * @param trigger_predicate Predicate function that will be called to check if the async update * method should be triggered. + * @param thread_priority Priority of the async update thread * * \note The parsed trigger_predicate should be free from any concurrency issues. It is expected * to be both thread-safe and reentrant. From e2b09243f2a49a90cc5a3a0e094f85cec94d0288 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 17 Jul 2024 15:00:00 +0200 Subject: [PATCH 12/13] renamed methods and variable to be more generic --- .../realtime_tools/async_function_handler.hpp | 119 +++++++++--------- test/test_async_function_handler.cpp | 18 +-- 2 files changed, 69 insertions(+), 68 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index f4ed63ea..d8d46809 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -47,38 +47,38 @@ class AsyncFunctionHandler public: AsyncFunctionHandler() = default; - ~AsyncFunctionHandler() { stop_async_update(); } + ~AsyncFunctionHandler() { stop_thread(); } - /// Initialize the AsyncFunctionHandler with the async_function and thread_priority + /// Initialize the AsyncFunctionHandler with the callback and thread_priority /** - * @param async_function Function that will be called asynchronously + * @param callback 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 async_function, + std::function callback, int thread_priority = 50) { - if (async_function == nullptr) { + if (callback == 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!"); + "running. Please stop the async callback first!"); } - async_function_ = async_function; + async_function_ = callback; thread_priority_ = thread_priority; } - /// Initialize the AsyncFunctionHandler with the async_function and trigger_predicate + /// Initialize the AsyncFunctionHandler with the callback, trigger_predicate and thread_priority /** - * @param async_function Function that will be called asynchronously - * @param trigger_predicate Predicate function that will be called to check if the async update - * method should be triggered. - * @param thread_priority Priority of the async update thread + * @param callback Function that will be called asynchronously. + * @param trigger_predicate Predicate function that will be called to check if the async callback + * method should be triggered or not. + * @param thread_priority Priority of the async worker thread. * * \note The parsed trigger_predicate should be free from any concurrency issues. It is expected * to be both thread-safe and reentrant. @@ -88,33 +88,33 @@ class AsyncFunctionHandler * If the parsed functions are not valid, it will throw a runtime error. */ void init( - std::function async_function, + std::function callback, std::function trigger_predicate, int thread_priority = 50) { if (trigger_predicate == nullptr) { throw std::runtime_error("AsyncFunctionHandler: parsed trigger predicate is not valid!"); } - init(async_function, thread_priority); + init(callback, thread_priority); trigger_predicate_ = trigger_predicate; } - /// Triggers the async update method cycle + /// Triggers the async callback 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 + * @return A pair with the first element being a boolean indicating if the async callback 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. + * If the callback method is waiting for the trigger, it will notify the async thread to start + * the callback. + * If the async callback method is still running, it will return the last return value from the + * last trigger cycle. * * \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( + std::pair trigger_async_callback( const rclcpp::Time & time, const rclcpp::Duration & period) { if (!is_initialized()) { @@ -122,7 +122,7 @@ class AsyncFunctionHandler } if (!is_running()) { throw std::runtime_error( - "AsyncFunctionHandler: need to start the async update thread first before triggering!"); + "AsyncFunctionHandler: need to start the async callback thread first before triggering!"); } std::unique_lock lock(async_mtx_, std::try_to_lock); bool trigger_status = false; @@ -130,17 +130,17 @@ class AsyncFunctionHandler { std::unique_lock scoped_lock(std::move(lock)); trigger_in_progress_ = true; - current_update_time_ = time; - current_update_period_ = period; + current_callback_time_ = time; + current_callback_period_ = period; } - async_update_condition_.notify_one(); + async_callback_condition_.notify_one(); trigger_status = true; } - const T return_value = async_update_return_; + const T return_value = async_callback_return_; return std::make_pair(trigger_status, return_value); } - /// Waits until the current async update method cycle to finish + /// Waits until the current async callback method trigger cycle is finished /** * If the async method is running, it will wait for the current async method call to finish. */ @@ -158,74 +158,74 @@ class AsyncFunctionHandler */ bool is_initialized() const { return async_function_ && trigger_predicate_; } - /// Join the async update thread + /// Join the async callback 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() + void join_async_callback_thread() { if (is_running()) { thread_.join(); } } - /// Check if the async update thread is running + /// Check if the async worker thread is running /** - * @return True if the async update thread is running, false otherwise + * @return True if the async worker thread is running, false otherwise */ bool is_running() const { return thread_.joinable(); } - /// Check if the async update is triggered to stop the cycle + /// Check if the async callback is triggered to stop the cycle /** - * @return True if the async update is stopped, false otherwise + * @return True if the async callback is requested to be stopped, false otherwise */ - bool is_stopped() const { return async_update_stop_; } + bool is_stopped() const { return stop_async_callback_; } - /// Get the async update thread + /// Get the async worker thread /** - * @return The async update thread + * @return The async callback thread */ - std::thread & get_async_thread() { return thread_; } + std::thread & get_thread() { return thread_; } - /// Check if the async update method is in progress + /// Check if the async callback method is in progress /** - * @return True if the async update method is in progress, false otherwise + * @return True if the async callback method is in progress, false otherwise */ bool is_trigger_cycle_in_progress() const { return trigger_in_progress_; } - /// Stops the async update thread + /// Stops the callback 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() + void stop_thread() { if (is_running()) { { std::unique_lock lock(async_mtx_); - async_update_stop_ = true; + stop_async_callback_ = true; } - async_update_condition_.notify_one(); + async_callback_condition_.notify_one(); thread_.join(); } } - /// Initialize the async update thread + /// Initializes and starts the callback 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 + * If the worker thread is not running, it will start the async callback thread. + * If the worker thread is already configured and running, does nothing and returns * immediately. */ - void start_async_update_thread() + void start_thread() { if (!is_initialized()) { throw std::runtime_error("AsyncFunctionHandler: need to be initialized first!"); } if (!thread_.joinable()) { - async_update_stop_ = false; + stop_async_callback_ = false; trigger_in_progress_ = false; - async_update_return_ = T(); + async_callback_return_ = T(); thread_ = std::thread([this]() -> void { if (!realtime_tools::configure_sched_fifo(thread_priority_)) { RCLCPP_WARN( @@ -237,13 +237,14 @@ class AsyncFunctionHandler "for details."); } - while (!async_update_stop_.load(std::memory_order_relaxed)) { + while (!stop_async_callback_.load(std::memory_order_relaxed)) { { 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_); + async_callback_condition_.wait( + lock, [this] { return trigger_in_progress_ || stop_async_callback_; }); + if (!stop_async_callback_) { + async_callback_return_ = + async_function_(current_callback_time_, current_callback_period_); } trigger_in_progress_ = false; } @@ -254,8 +255,8 @@ class AsyncFunctionHandler } private: - rclcpp::Time current_update_time_; - rclcpp::Duration current_update_period_{0, 0}; + rclcpp::Time current_callback_time_; + rclcpp::Duration current_callback_period_{0, 0}; std::function async_function_; std::function trigger_predicate_ = []() { return true; }; @@ -263,10 +264,10 @@ class AsyncFunctionHandler // Async related variables std::thread thread_; int thread_priority_ = std::numeric_limits::quiet_NaN(); - std::atomic_bool async_update_stop_{false}; + std::atomic_bool stop_async_callback_{false}; std::atomic_bool trigger_in_progress_{false}; - std::atomic async_update_return_; - std::condition_variable async_update_condition_; + std::atomic async_callback_return_; + std::condition_variable async_callback_condition_; std::condition_variable cycle_end_condition_; std::mutex async_mtx_; }; diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 0139a7d8..105ab7a4 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -35,7 +35,7 @@ void TestAsyncFunctionHandler::initialize() std::pair TestAsyncFunctionHandler::trigger() { - return handler_.trigger_async_update(last_callback_time_, last_callback_period_); + return handler_.trigger_async_callback(last_callback_time_, last_callback_period_); } return_type TestAsyncFunctionHandler::update( @@ -88,7 +88,7 @@ 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(); + async_class.get_handler().start_thread(); auto trigger_status = async_class.trigger(); ASSERT_TRUE(trigger_status.first); ASSERT_EQ(realtime_tools::return_type::OK, trigger_status.second); @@ -114,7 +114,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) 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(); + async_class.get_handler().start_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); auto trigger_status = async_class.trigger(); @@ -123,7 +123,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()); - ASSERT_TRUE(async_class.get_handler().get_async_thread().joinable()); + ASSERT_TRUE(async_class.get_handler().get_thread().joinable()); ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); @@ -136,7 +136,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().stop_async_update(); + async_class.get_handler().stop_thread(); ASSERT_LE(async_class.get_counter(), 2); // now the async update should be preempted @@ -153,7 +153,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(); + async_class.get_handler().start_thread(); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); @@ -176,7 +176,7 @@ TEST_F(AsyncFunctionHandlerTest, trigger_for_several_cycles) // Make sure that the failed triggers are less than 0.1% ASSERT_LT(missed_triggers, static_cast(0.001 * total_cycles)) << "The missed triggers cannot be more than 0.1%!"; - async_class.get_handler().stop_async_update(); + async_class.get_handler().stop_thread(); // now the async update should be preempted ASSERT_FALSE(async_class.get_handler().is_running()); @@ -195,7 +195,7 @@ 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_FALSE(async_class.get_handler().is_stopped()); - async_class.get_handler().start_async_update_thread(); + async_class.get_handler().start_thread(); ASSERT_TRUE(async_class.get_handler().is_running()); ASSERT_FALSE(async_class.get_handler().is_stopped()); EXPECT_EQ(async_class.get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); @@ -252,7 +252,7 @@ TEST_F(AsyncFunctionHandlerTest, test_with_deactivate_and_activate_cycles) ASSERT_FALSE(async_class.get_handler().is_stopped()); // now the async update should be preempted - async_class.get_handler().stop_async_update(); + async_class.get_handler().stop_thread(); ASSERT_FALSE(async_class.get_handler().is_running()); ASSERT_TRUE(async_class.get_handler().is_stopped()); } From 305f3b80c568393494121b34f8870f7999e0b628 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 7 Aug 2024 18:18:46 +0200 Subject: [PATCH 13/13] add get last execution time retrieval time --- include/realtime_tools/async_function_handler.hpp | 11 +++++++++++ test/test_async_function_handler.cpp | 7 +++---- test/test_async_function_handler.hpp | 2 -- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/include/realtime_tools/async_function_handler.hpp b/include/realtime_tools/async_function_handler.hpp index d8d46809..381e8e07 100644 --- a/include/realtime_tools/async_function_handler.hpp +++ b/include/realtime_tools/async_function_handler.hpp @@ -211,6 +211,12 @@ class AsyncFunctionHandler } } + /// Get the last execution time of the async callback method + /** + * @return The last execution time of the async callback method in seconds + */ + double get_last_execution_time() const { return last_execution_time_; } + /// Initializes and starts the callback thread /** * If the worker thread is not running, it will start the async callback thread. @@ -225,6 +231,7 @@ class AsyncFunctionHandler if (!thread_.joinable()) { stop_async_callback_ = false; trigger_in_progress_ = false; + last_execution_time_ = 0.0; async_callback_return_ = T(); thread_ = std::thread([this]() -> void { if (!realtime_tools::configure_sched_fifo(thread_priority_)) { @@ -243,8 +250,11 @@ class AsyncFunctionHandler async_callback_condition_.wait( lock, [this] { return trigger_in_progress_ || stop_async_callback_; }); if (!stop_async_callback_) { + const auto start_time = std::chrono::steady_clock::now(); async_callback_return_ = async_function_(current_callback_time_, current_callback_period_); + const auto end_time = std::chrono::steady_clock::now(); + last_execution_time_ = std::chrono::duration(end_time - start_time).count(); } trigger_in_progress_ = false; } @@ -270,6 +280,7 @@ class AsyncFunctionHandler std::condition_variable async_callback_condition_; std::condition_variable cycle_end_condition_; std::mutex async_mtx_; + std::atomic last_execution_time_; }; } // namespace realtime_tools diff --git a/test/test_async_function_handler.cpp b/test/test_async_function_handler.cpp index 105ab7a4..eb0e11b1 100644 --- a/test/test_async_function_handler.cpp +++ b/test/test_async_function_handler.cpp @@ -35,14 +35,12 @@ void TestAsyncFunctionHandler::initialize() std::pair TestAsyncFunctionHandler::trigger() { - return handler_.trigger_async_callback(last_callback_time_, last_callback_period_); + return handler_.trigger_async_callback(rclcpp::Time(0, 0), rclcpp::Duration(0, 0)); } return_type TestAsyncFunctionHandler::update( - const rclcpp::Time & time, const rclcpp::Duration & period) + 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_++; @@ -126,6 +124,7 @@ TEST_F(AsyncFunctionHandlerTest, check_triggering) ASSERT_TRUE(async_class.get_handler().get_thread().joinable()); ASSERT_TRUE(async_class.get_handler().is_trigger_cycle_in_progress()); async_class.get_handler().wait_for_trigger_cycle_to_finish(); + async_class.get_handler().get_last_execution_time(); ASSERT_FALSE(async_class.get_handler().is_trigger_cycle_in_progress()); ASSERT_EQ(async_class.get_counter(), 1); diff --git a/test/test_async_function_handler.hpp b/test/test_async_function_handler.hpp index e66af6bd..c4805436 100644 --- a/test/test_async_function_handler.hpp +++ b/test/test_async_function_handler.hpp @@ -52,8 +52,6 @@ class TestAsyncFunctionHandler private: rclcpp_lifecycle::State state_; - rclcpp::Time last_callback_time_; - rclcpp::Duration last_callback_period_{0, 0}; int counter_; return_type return_state_; realtime_tools::AsyncFunctionHandler handler_;