Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Async Function Handler for Controllers #1489

Merged
merged 91 commits into from
Oct 17, 2024
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
91 commits
Select commit Hold shift + click to select a range
e8130a4
Add first version of the async controllers
saikishor Apr 9, 2024
062a445
removed extra notify one
saikishor Apr 10, 2024
fd270ca
added async_update_ready_ to use with conditional variable to avoid s…
saikishor Apr 10, 2024
703f91f
call notify_one after setting the async_update_ready
saikishor Apr 10, 2024
c09339f
add precommit changes
saikishor Apr 10, 2024
1f58a2d
add missing this in the conditional variable callback
saikishor Apr 10, 2024
c2f7192
change atomic bool to normal bool
saikishor Apr 10, 2024
4255cc3
add wait_for_update_to_finish method
saikishor Apr 10, 2024
9f7c0b4
add notify_one in different places to wait properly to finish update …
saikishor Apr 11, 2024
faba7cd
added unique lock and check with try_lock for triggering calls
saikishor Apr 11, 2024
4ffa885
use try_to_lock for better checking of owning the lock or not
saikishor Apr 11, 2024
f575440
add current update time and period to be able to update them
saikishor Apr 11, 2024
4348019
update documentation of the async method
saikishor Apr 11, 2024
a2469ca
added some comments about the issue with the get_state method
saikishor Apr 11, 2024
ffaad19
use atomic_bool
saikishor Apr 11, 2024
b7586a2
minor change in the logic
saikishor Apr 11, 2024
0772c49
initialize the duration to fix the compilation error
saikishor Apr 11, 2024
bf2a2d3
Move the main async logic into a separate method
saikishor Apr 11, 2024
89e6a7b
Add async function handler to handle parsed functions
saikishor Apr 11, 2024
2e0a0d2
add missing template typenames on functions
saikishor Apr 12, 2024
bfae6a1
set wait_for_update_to_finish to const
saikishor Apr 12, 2024
c2adc42
change async_update_ready_ to atomic bool
saikishor Apr 12, 2024
5ca2e5d
use also async_update_stop_ also in the conditional wait predicate fu…
saikishor Apr 12, 2024
c5a00e6
Add some utility methods
saikishor Apr 12, 2024
b83157d
modify the std::function arguments of async_update method
saikishor Apr 12, 2024
225c96f
Fix the issue with the overidding template names and typo in the std:…
saikishor Apr 13, 2024
1f3377c
fix the wait for update to finish method
saikishor Apr 13, 2024
fd8be6d
Check if the parsed functions are valid or not in the init method
saikishor Apr 14, 2024
794a8c0
unlock the mutex before notifying the conditional variable
saikishor Apr 14, 2024
619f46d
Do not check if the thread is joinable in the is_initialized method
saikishor Apr 14, 2024
98aa3b0
added a explicit method to join the async thread
saikishor Apr 14, 2024
7f9e47f
Add missing includes
saikishor Apr 14, 2024
b5ad675
change the way the initialization in done
saikishor Apr 14, 2024
3cd856f
Added tests for the new async function handler
saikishor Apr 14, 2024
f88fe20
extend the tests with another loop
saikishor Apr 14, 2024
eb7da6b
Added test case to check for triggering
saikishor Apr 14, 2024
b61b8cd
Also test the case of triggering without initialization
saikishor Apr 14, 2024
3fe3d31
Add test triggering the handler for several cycles
saikishor Apr 14, 2024
1fb5955
Add the test cases of changes in the lifecycle state
saikishor Apr 14, 2024
6952577
remove is_async method and added is_preempted method + changes to the…
saikishor Apr 14, 2024
b3975db
Use the AsyncFunctionHandler instead of implementing in the controlle…
saikishor Apr 15, 2024
47d5928
Add documentation to the methods
saikishor Apr 15, 2024
d86be02
remove the arguments to initialize_async_update_thread method
saikishor Apr 15, 2024
76e3f30
simplify the is_initialized method check
saikishor Apr 15, 2024
f1634bc
rename preempt_async_update to stop_async_update
saikishor Apr 15, 2024
c094f81
Separate the exceptions for the parsed methods
saikishor Apr 15, 2024
88ceb3c
change the exception inside the initialize_async_update_thread
saikishor Apr 15, 2024
cecb00b
move the test implementations to the cpp file
saikishor Apr 15, 2024
f61e382
Remove the unlock when preempted as the lock goes out of scope
saikishor Apr 15, 2024
c070ed6
rename async_update_ready_ to trigger_in_progress_
saikishor Apr 15, 2024
fff989e
update the docs
saikishor Apr 15, 2024
71f7ad9
rename the method wait_for_update_to_finish to wait_for_trigger_cycle…
saikishor Apr 15, 2024
fa6a88c
Add documentation to the trigger_update method and add it to the cont…
saikishor Apr 15, 2024
91aab1f
added a method to stop the async update cycle before deactivating the…
saikishor Apr 15, 2024
6ad0c1b
return std::pair to check if the trigger is successful or not
saikishor Apr 16, 2024
d2c6692
update the runtime error throw message
saikishor Apr 16, 2024
c4b9f82
rename is_preempted to is_stopped for the context of the threads
saikishor Apr 18, 2024
48b2c2f
change EXPECT_THROW to ASSERT_THROW
saikishor Apr 18, 2024
a62da78
Update the documentation of the init return entity
saikishor Apr 18, 2024
f8a93ff
Fix the doc of stop_async_update_cycle
saikishor Apr 18, 2024
5d4b4e2
add minor information
saikishor Apr 18, 2024
20bc5d1
add missing functional include
saikishor Apr 18, 2024
600da5c
check if lock is owned or not before in the condition
saikishor Apr 21, 2024
7f2aa64
Use separated arguments for better clarity
saikishor Apr 21, 2024
679fbf9
Move the lock within a scope to avoid exclusive unlock within the thread
saikishor Apr 22, 2024
d1ac8df
notify other waiting threads before stopping
saikishor Apr 22, 2024
4e3dbb1
Scope the lock to avoid manual unlock in the trigger_async_update method
saikishor Apr 22, 2024
30be210
Update async_update_stop_ within the scope of the lock to avoid missi…
saikishor Apr 22, 2024
e54e613
Add new conditional variable to have unexpected behavior when working…
saikishor Apr 22, 2024
47f6b02
Changes for the starting thread upon activation
saikishor Apr 21, 2024
8525725
remove unused notify at the end of the thread
saikishor Apr 22, 2024
4eb74cc
simplify the logic inside the thread
saikishor May 17, 2024
a131029
add pre-commit formatting changes
saikishor May 17, 2024
b4b2afa
remove the AsyncControllerThread integration to replace it with Async…
saikishor May 17, 2024
ee64bc7
move the async function handler to the realtime_tools and integrate f…
saikishor Jun 12, 2024
469682a
start the async thread when configuring the controller
saikishor Jun 12, 2024
f3b5d43
remove the get_state bind for API change
saikishor Jun 13, 2024
408780c
add new API naming changes
saikishor Jul 17, 2024
8f10adc
added thread priority argument to be able to set the scheduler priority
saikishor Aug 5, 2024
a40b005
stop the thread on cleanup of the controller
saikishor Sep 13, 2024
aa9a81e
wait for cycle to finish in the switch controllers before deactivatin…
saikishor Sep 13, 2024
023a204
fix the thread_priority parameter declaration type
saikishor Sep 13, 2024
3bd19bf
change conditioning to not trigger logging for a failed return as well
saikishor Sep 13, 2024
ebb69c5
Add tests for the async controller
saikishor Sep 13, 2024
e38f567
Update controller_interface/src/controller_interface_base.cpp
saikishor Oct 17, 2024
cbc3638
Merge branch 'master' into async_controllers
saikishor Oct 17, 2024
00228c4
Update controller_interface/include/controller_interface/controller_i…
destogl Oct 17, 2024
df9378f
Fixing pre-commit.
destogl Oct 17, 2024
dd89180
skip triggeting update cycle before deactivating the async controller
saikishor Oct 17, 2024
5a85a24
fix the testing for the recent change in the deactivation scheme
saikishor Oct 17, 2024
75754de
Add controller update stats to print every 20 seconds when there is a…
saikishor Oct 17, 2024
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Add tests for the async controller
  • Loading branch information
saikishor committed Sep 26, 2024
commit ebb69c58a7121f66c6f88ca9b536e62a21fd0ee9
4 changes: 4 additions & 0 deletions controller_manager/test/test_controller/test_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,10 @@ controller_interface::InterfaceConfiguration TestController::state_interface_con
controller_interface::return_type TestController::update(
const rclcpp::Time & /*time*/, const rclcpp::Duration & period)
{
if (is_async())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1000 / (2 * get_update_rate())));
}
update_period_ = period;
++internal_counter;

Expand Down
185 changes: 185 additions & 0 deletions controller_manager/test/test_controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -232,6 +232,191 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle)
EXPECT_EQ(1, test_controller.use_count());
}

TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
{
const auto test_param = GetParam();
auto test_controller = std::make_shared<test_controller::TestController>();
auto test_controller2 = std::make_shared<test_controller::TestController>();
constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
cm_->add_controller(
test_controller, test_controller::TEST_CONTROLLER_NAME,
test_controller::TEST_CONTROLLER_CLASS_NAME);
cm_->add_controller(
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
EXPECT_EQ(2, test_controller.use_count());

// setup interface to claim from controllers
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
{
cmd_itfs_cfg.names.push_back(interface);
}
test_controller->set_command_interface_configuration(cmd_itfs_cfg);

controller_interface::InterfaceConfiguration state_itfs_cfg;
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
{
state_itfs_cfg.names.push_back(interface);
}
test_controller->set_state_interface_configuration(state_itfs_cfg);

controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
{
cmd_itfs_cfg2.names.push_back(interface);
}
test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);

controller_interface::InterfaceConfiguration state_itfs_cfg2;
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
test_controller2->set_state_interface_configuration(state_itfs_cfg2);

// Check if namespace is set correctly
RCLCPP_INFO(
rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'",
cm_->get_namespace());
EXPECT_STREQ(cm_->get_namespace(), "/");
RCLCPP_INFO(
rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'",
test_controller->get_node()->get_namespace());
EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/");
RCLCPP_INFO(
rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'",
test_controller2->get_node()->get_namespace());
EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/");

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter)
<< "Update should not reach an unconfigured controller";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());

// configure controller
rclcpp::Parameter update_rate_parameter("update_rate", static_cast<int>(20));
rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true));
test_controller->get_node()->set_parameter(update_rate_parameter);
test_controller->get_node()->set_parameter(is_async_parameter);
{
ControllerManagerRunner cm_runner(this);
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
cm_->configure_controller(TEST_CONTROLLER2_NAME);
}
EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());

// Start controller, will take effect at the end of the update function
std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(test_param.expected_return, switch_future.get());
}

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);

// Start the real test controller, will take effect at the end of the update function
start_controllers = {test_controller::TEST_CONTROLLER_NAME};
stop_controllers = {};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}
EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(test_controller->internal_counter, 0u);
std::this_thread::sleep_for(
std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
EXPECT_EQ(test_controller->internal_counter, 1u);
size_t last_internal_counter = test_controller->internal_counter;

// Stop controller, will take effect at the end of the update function
start_controllers = {};
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));

ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";

EXPECT_EQ(
controller_interface::return_type::OK,
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
EXPECT_EQ(last_internal_counter, test_controller->internal_counter)
<< "This shouldn't have updated as this is async and in the controller it is waiting before "
"updating the counter";
std::this_thread::sleep_for(
std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
EXPECT_EQ(last_internal_counter + 1u, test_controller->internal_counter)
<< "Controller is stopped at the end of update, so it should have done one more update";
{
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
}

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
test_controller->get_lifecycle_state().id());
auto unload_future = std::async(
std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
test_controller::TEST_CONTROLLER_NAME);

ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
<< "unload_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());

EXPECT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
test_controller->get_lifecycle_state().id());
EXPECT_EQ(1, test_controller.use_count());
}

TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
{
auto strictness = GetParam().strictness;
Expand Down
Loading