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 cb5b092a6e..f52f431111 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" @@ -286,7 +286,7 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy private: std::shared_ptr node_; - std::unique_ptr> async_handler_; + std::unique_ptr> async_handler_; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; diff --git a/controller_interface/package.xml b/controller_interface/package.xml index 9a99449043..db89bbc12e 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 2bdcdbf519..63a8c061f5 100644 --- a/controller_interface/src/controller_interface_base.cpp +++ b/controller_interface/src/controller_interface_base.cpp @@ -97,7 +97,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_