Skip to content

Commit 772c788

Browse files
saikishorsaikishor
committed
Add test for this case
1 parent 0086370 commit 772c788

File tree

1 file changed

+220
-0
lines changed

1 file changed

+220
-0
lines changed

‎controller_manager/test/test_controller_manager.cpp‎

Lines changed: 220 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -391,6 +391,17 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
391391
testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period))));
392392
size_t last_internal_counter = test_controller->internal_counter;
393393

394+
{
395+
ControllerManagerRunner cm_runner(this);
396+
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
397+
}
398+
ASSERT_THAT(
399+
test_controller->internal_counter,
400+
testing::AllOf(
401+
testing::Ge(last_internal_counter + 18), testing::Le(last_internal_counter + 21)))
402+
<< "As the sleep is 1 sec and the controller rate is 20Hz, we should have approx. 20 updates";
403+
404+
last_internal_counter = test_controller->internal_counter;
394405
// Stop controller, will take effect at the end of the update function
395406
start_controllers = {};
396407
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
@@ -434,6 +445,215 @@ TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle)
434445
EXPECT_EQ(1, test_controller.use_count());
435446
}
436447

448+
TEST_P(TestControllerManagerWithStrictness, async_controller_lifecycle_at_cm_rate)
449+
{
450+
const auto test_param = GetParam();
451+
auto test_controller = std::make_shared<test_controller::TestController>();
452+
auto test_controller2 = std::make_shared<test_controller::TestController>();
453+
constexpr char TEST_CONTROLLER2_NAME[] = "test_controller2_name";
454+
cm_->add_controller(
455+
test_controller, test_controller::TEST_CONTROLLER_NAME,
456+
test_controller::TEST_CONTROLLER_CLASS_NAME);
457+
cm_->add_controller(
458+
test_controller2, TEST_CONTROLLER2_NAME, test_controller::TEST_CONTROLLER_CLASS_NAME);
459+
EXPECT_EQ(2u, cm_->get_loaded_controllers().size());
460+
EXPECT_EQ(2, test_controller.use_count());
461+
462+
// setup interface to claim from controllers
463+
controller_interface::InterfaceConfiguration cmd_itfs_cfg;
464+
cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
465+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES)
466+
{
467+
cmd_itfs_cfg.names.push_back(interface);
468+
}
469+
test_controller->set_command_interface_configuration(cmd_itfs_cfg);
470+
471+
controller_interface::InterfaceConfiguration state_itfs_cfg;
472+
state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL;
473+
for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES)
474+
{
475+
state_itfs_cfg.names.push_back(interface);
476+
}
477+
for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES)
478+
{
479+
state_itfs_cfg.names.push_back(interface);
480+
}
481+
test_controller->set_state_interface_configuration(state_itfs_cfg);
482+
483+
controller_interface::InterfaceConfiguration cmd_itfs_cfg2;
484+
cmd_itfs_cfg2.type = controller_interface::interface_configuration_type::INDIVIDUAL;
485+
for (const auto & interface : ros2_control_test_assets::TEST_SYSTEM_HARDWARE_COMMAND_INTERFACES)
486+
{
487+
cmd_itfs_cfg2.names.push_back(interface);
488+
}
489+
test_controller2->set_command_interface_configuration(cmd_itfs_cfg2);
490+
491+
controller_interface::InterfaceConfiguration state_itfs_cfg2;
492+
state_itfs_cfg2.type = controller_interface::interface_configuration_type::ALL;
493+
test_controller2->set_state_interface_configuration(state_itfs_cfg2);
494+
495+
// Check if namespace is set correctly
496+
RCLCPP_INFO(
497+
rclcpp::get_logger("test_controller_manager"), "Controller Manager namespace is '%s'",
498+
cm_->get_namespace());
499+
EXPECT_STREQ(cm_->get_namespace(), "/");
500+
RCLCPP_INFO(
501+
rclcpp::get_logger("test_controller_manager"), "Controller 1 namespace is '%s'",
502+
test_controller->get_node()->get_namespace());
503+
EXPECT_STREQ(test_controller->get_node()->get_namespace(), "/");
504+
RCLCPP_INFO(
505+
rclcpp::get_logger("test_controller_manager"), "Controller 2 namespace is '%s'",
506+
test_controller2->get_node()->get_namespace());
507+
EXPECT_STREQ(test_controller2->get_node()->get_namespace(), "/");
508+
509+
EXPECT_EQ(
510+
controller_interface::return_type::OK,
511+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
512+
EXPECT_EQ(0u, test_controller->internal_counter)
513+
<< "Update should not reach an unconfigured controller";
514+
515+
EXPECT_EQ(
516+
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
517+
test_controller->get_lifecycle_state().id());
518+
519+
// configure controller
520+
rclcpp::Parameter is_async_parameter("is_async", rclcpp::ParameterValue(true));
521+
test_controller->get_node()->set_parameter(is_async_parameter);
522+
{
523+
ControllerManagerRunner cm_runner(this);
524+
cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME);
525+
cm_->configure_controller(TEST_CONTROLLER2_NAME);
526+
}
527+
EXPECT_EQ(
528+
controller_interface::return_type::OK,
529+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
530+
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
531+
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is not started";
532+
533+
EXPECT_EQ(
534+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
535+
test_controller->get_lifecycle_state().id());
536+
537+
// Start controller, will take effect at the end of the update function
538+
std::vector<std::string> start_controllers = {"fake_controller", TEST_CONTROLLER2_NAME};
539+
std::vector<std::string> stop_controllers = {};
540+
auto switch_future = std::async(
541+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
542+
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
543+
544+
EXPECT_EQ(
545+
controller_interface::return_type::OK,
546+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
547+
EXPECT_EQ(0u, test_controller2->internal_counter) << "Controller is started at the end of update";
548+
{
549+
ControllerManagerRunner cm_runner(this);
550+
EXPECT_EQ(test_param.expected_return, switch_future.get());
551+
}
552+
553+
EXPECT_EQ(
554+
controller_interface::return_type::OK,
555+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
556+
EXPECT_GE(test_controller2->internal_counter, test_param.expected_counter);
557+
558+
// Start the real test controller, will take effect at the end of the update function
559+
start_controllers = {test_controller::TEST_CONTROLLER_NAME};
560+
stop_controllers = {};
561+
switch_future = std::async(
562+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
563+
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
564+
565+
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
566+
<< "switch_controller should be blocking until next update cycle";
567+
568+
EXPECT_EQ(
569+
controller_interface::return_type::OK,
570+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
571+
EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is started at the end of update";
572+
{
573+
ControllerManagerRunner cm_runner(this);
574+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
575+
}
576+
EXPECT_EQ(
577+
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller->get_lifecycle_state().id());
578+
579+
time_ = cm_->get_clock()->now();
580+
EXPECT_EQ(
581+
controller_interface::return_type::OK,
582+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
583+
EXPECT_GE(test_controller->internal_counter, 1u);
584+
auto current_counter = test_controller->internal_counter;
585+
std::this_thread::sleep_for(
586+
std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
587+
EXPECT_EQ(test_controller->internal_counter, current_counter + 1u);
588+
EXPECT_NEAR(test_controller->update_period_.seconds(), 0.01, 0.008);
589+
590+
const double exp_period = (cm_->get_clock()->now() - time_).seconds();
591+
time_ = cm_->get_clock()->now();
592+
EXPECT_EQ(
593+
controller_interface::return_type::OK,
594+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
595+
EXPECT_EQ(test_controller->internal_counter, current_counter + 1u);
596+
std::this_thread::sleep_for(
597+
std::chrono::milliseconds(1000 / (test_controller->get_update_rate())));
598+
EXPECT_EQ(test_controller->internal_counter, current_counter + 2u);
599+
EXPECT_THAT(
600+
test_controller->update_period_.seconds(),
601+
testing::AllOf(testing::Gt(0.6 * exp_period), testing::Lt((1.4 * exp_period))));
602+
size_t last_internal_counter = test_controller->internal_counter;
603+
604+
{
605+
ControllerManagerRunner cm_runner(this);
606+
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
607+
}
608+
ASSERT_THAT(
609+
test_controller->internal_counter,
610+
testing::AllOf(
611+
testing::Ge(last_internal_counter + cm_->get_update_rate() - 3),
612+
testing::Le(last_internal_counter + cm_->get_update_rate() + 1)))
613+
<< "As the sleep is 1 sec and the controller rate is 100Hz, we should have approx. 100 updates";
614+
615+
// Sleep for 2 cycles to allow for any changes
616+
std::this_thread::sleep_for(std::chrono::milliseconds(20));
617+
last_internal_counter = test_controller->internal_counter;
618+
{
619+
ControllerManagerRunner cm_runner(this);
620+
// Stop controller, will take effect at the end of the update function
621+
start_controllers = {};
622+
stop_controllers = {test_controller::TEST_CONTROLLER_NAME};
623+
switch_future = std::async(
624+
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
625+
start_controllers, stop_controllers, test_param.strictness, true, rclcpp::Duration(0, 0));
626+
627+
ASSERT_EQ(std::future_status::ready, switch_future.wait_for(std::chrono::milliseconds(100)))
628+
<< "switch_controller should be blocking until next update cycle";
629+
630+
std::this_thread::sleep_for(std::chrono::milliseconds(20));
631+
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
632+
EXPECT_EQ(last_internal_counter + 1, test_controller->internal_counter)
633+
<< "Controller is stopped at the end of update, it should finish it's active cycle";
634+
}
635+
636+
EXPECT_EQ(
637+
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
638+
test_controller->get_lifecycle_state().id());
639+
auto unload_future = std::async(
640+
std::launch::async, &controller_manager::ControllerManager::unload_controller, cm_,
641+
test_controller::TEST_CONTROLLER_NAME);
642+
643+
ASSERT_EQ(std::future_status::timeout, unload_future.wait_for(std::chrono::milliseconds(100)))
644+
<< "unload_controller should be blocking until next update cycle";
645+
646+
EXPECT_EQ(
647+
controller_interface::return_type::OK,
648+
cm_->update(time_, rclcpp::Duration::from_seconds(0.01)));
649+
EXPECT_EQ(controller_interface::return_type::OK, unload_future.get());
650+
651+
EXPECT_EQ(
652+
lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
653+
test_controller->get_lifecycle_state().id());
654+
EXPECT_EQ(1, test_controller.use_count());
655+
}
656+
437657
TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate)
438658
{
439659
auto strictness = GetParam().strictness;

0 commit comments

Comments
 (0)