From 52218f87fe058758f7799ff47d831b9dd1fcae81 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 4 Oct 2023 12:57:10 +0200 Subject: [PATCH] Fix next controller update cycle time clock (#1127) --- controller_manager/src/controller_manager.cpp | 16 +++++--- .../test/controller_manager_test_common.hpp | 13 ++++--- .../test/test_controller_manager.cpp | 39 +++++++++---------- ...roller_manager_hardware_error_handling.cpp | 34 ++++++++-------- ...llers_chaining_with_controller_manager.cpp | 2 +- .../test/test_spawner_unspawner.cpp | 6 +-- 6 files changed, 58 insertions(+), 52 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6bb37a13b0..a48069acfb 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -565,7 +565,8 @@ controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::load_c controller_spec.c = controller; controller_spec.info.name = controller_name; controller_spec.info.type = controller_type; - controller_spec.next_update_cycle_time = std::make_shared(0); + controller_spec.next_update_cycle_time = std::make_shared( + 0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); return add_controller_impl(controller_spec); } @@ -1457,7 +1458,8 @@ void ControllerManager::activate_controllers( auto controller = found_it->c; auto controller_name = found_it->info.name; // reset the next update cycle time for newly activated controllers - *found_it->next_update_cycle_time = rclcpp::Time(0); + *found_it->next_update_cycle_time = + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type()); bool assignment_successful = true; // assign command interfaces to the controller @@ -2043,8 +2045,10 @@ controller_interface::return_type ControllerManager::update( run_controller_at_cm_rate ? period : rclcpp::Duration::from_seconds((1.0 / controller_update_rate)); - bool controller_go = (time == rclcpp::Time(0)) || - (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); + bool controller_go = + (time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) || + (time.seconds() >= loaded_controller.next_update_cycle_time->seconds()); RCLCPP_DEBUG( get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '", @@ -2055,7 +2059,9 @@ controller_interface::return_type ControllerManager::update( { auto controller_ret = loaded_controller.c->update(time, controller_period); - if (*loaded_controller.next_update_cycle_time == rclcpp::Time(0)) + if ( + *loaded_controller.next_update_cycle_time == + rclcpp::Time(0, 0, this->get_node_clock_interface()->get_clock()->get_clock_type())) { *loaded_controller.next_update_cycle_time = time; } diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 78c3fcb06b..7bf5cae628 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -41,7 +41,6 @@ namespace { -const auto TIME = rclcpp::Time(0); const auto PERIOD = rclcpp::Duration::from_seconds(0.01); const auto STRICT = controller_manager_msgs::srv::SwitchController::Request::STRICT; const auto BEST_EFFORT = controller_manager_msgs::srv::SwitchController::Request::BEST_EFFORT; @@ -101,6 +100,7 @@ class ControllerManagerFixture : public ::testing::Test cm_->robot_description_callback(msg); } } + time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } static void SetUpTestCase() { rclcpp::init(0, nullptr); } @@ -119,7 +119,7 @@ class ControllerManagerFixture : public ::testing::Test { while (run_updater_) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); @@ -157,6 +157,7 @@ class ControllerManagerFixture : public ::testing::Test bool run_updater_; const std::string robot_description_; const bool pass_urdf_as_parameter_; + rclcpp::Time time_; }; class TestControllerManagerSrvs @@ -177,9 +178,9 @@ class TestControllerManagerSrvs std::chrono::milliseconds(10), [&]() { - cm_->read(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); executor_->add_node(cm_); @@ -206,7 +207,7 @@ class TestControllerManagerSrvs while (service_executor.spin_until_future_complete(result, std::chrono::milliseconds(50)) != rclcpp::FutureReturnCode::SUCCESS) { - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); } } else diff --git a/controller_manager/test/test_controller_manager.cpp b/controller_manager/test/test_controller_manager.cpp index b4d66f9039..e317726585 100644 --- a/controller_manager/test/test_controller_manager.cpp +++ b/controller_manager/test/test_controller_manager.cpp @@ -96,7 +96,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -108,7 +108,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) cm_->configure_controller(TEST_CONTROLLER2_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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"; @@ -123,7 +123,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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); @@ -132,7 +132,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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 @@ -147,7 +147,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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); @@ -157,7 +157,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_GE(test_controller->internal_counter, 1u); size_t last_internal_counter = test_controller->internal_counter; @@ -173,7 +173,7 @@ TEST_P(TestControllerManagerWithStrictness, controller_lifecycle) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); 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"; { @@ -208,7 +208,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -220,7 +220,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -237,7 +237,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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); @@ -251,7 +251,7 @@ TEST_P(TestControllerManagerWithStrictness, per_controller_update_rate) { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); } EXPECT_GE(test_controller->internal_counter, 1u); EXPECT_EQ(test_controller->get_update_rate(), 4u); @@ -286,7 +286,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ(2, test_controller.use_count()); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Update should not reach an unconfigured controller"; @@ -299,7 +299,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -316,7 +316,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(last_internal_counter, test_controller->internal_counter) << "Controller is started at the end of update"; { @@ -332,7 +332,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd { EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); loop_rate.sleep(); } // if we do 2 times of the controller_manager update rate, the internal counter should be @@ -348,8 +348,7 @@ TEST_P(TestControllerManagerWithUpdateRates, per_controller_equal_and_higher_upd << "switch_controller should be blocking until next update cycle"; EXPECT_EQ( - controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))) + controller_interface::return_type::OK, cm_->update(time_, rclcpp::Duration::from_seconds(0.01))) << "Controller is stopped at the end of update, so it should have done one more update"; { ControllerManagerRunner cm_runner(this); @@ -393,7 +392,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) cm_->configure_controller(test_controller::TEST_CONTROLLER_NAME); EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01))); EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started"; EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller->get_state().id()); @@ -411,7 +410,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) EXPECT_EQ( controller_interface::return_type::OK, - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01))); + 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); @@ -425,7 +424,7 @@ TEST_P(TestControllerUpdateRates, check_the_controller_update_rate) const auto controller_update_rate = test_controller->get_update_rate(); const auto initial_counter = test_controller->internal_counter; - rclcpp::Time time = rclcpp::Time(0); + rclcpp::Time time = time_; for (size_t update_counter = 0; update_counter <= 10 * cm_update_rate; ++update_counter) { EXPECT_EQ( diff --git a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp index 1ec725f8c5..20af889c81 100644 --- a/controller_manager/test/test_controller_manager_hardware_error_handling.cpp +++ b/controller_manager/test/test_controller_manager_hardware_error_handling.cpp @@ -138,7 +138,7 @@ class TestControllerManagerWithTestableCM cm_->configure_controller(TEST_BROADCASTER_ALL_NAME); cm_->configure_controller(TEST_BROADCASTER_SENSOR_NAME); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(0u, test_controller_actuator->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_controller_system->internal_counter) << "Controller is not started"; EXPECT_EQ(0u, test_broadcaster_all->internal_counter) << "Controller is not started"; @@ -226,7 +226,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -249,7 +249,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -261,7 +261,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er test_controller_actuator->set_first_command_interface_value_to = READ_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -278,7 +278,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -289,7 +289,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -333,7 +333,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -350,7 +350,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er auto previous_counter_higher = test_controller_system->internal_counter; auto new_counter = test_broadcaster_sensor->internal_counter + 1; - EXPECT_NO_THROW(cm_->read(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->read(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -361,7 +361,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_read_er EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has read error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) @@ -418,7 +418,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e hardware_interface::lifecycle_state_names::ACTIVE); { - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_GE(test_controller_actuator->internal_counter, 1u) << "Controller is started at the end of update"; EXPECT_GE(test_controller_system->internal_counter, 1u) @@ -441,7 +441,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // Execute first time without any errors { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) << "Execute without errors"; EXPECT_EQ(test_broadcaster_all->internal_counter, new_counter) << "Execute without errors"; @@ -453,7 +453,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e test_controller_actuator->set_first_command_interface_value_to = WRITE_FAIL_VALUE; { auto new_counter = test_controller_actuator->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, new_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -470,7 +470,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -481,7 +481,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter) << "Execute without errors to write value"; EXPECT_EQ(test_controller_system->internal_counter, new_counter) @@ -525,7 +525,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e auto previous_counter_lower = test_controller_actuator->internal_counter + 1; auto previous_counter_higher = test_controller_system->internal_counter + 1; - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute without errors to write value"; @@ -544,7 +544,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e // here happens error in hardware and // "actuator controller" and "broadcaster all" are deactivated - EXPECT_NO_THROW(cm_->write(TIME, PERIOD)); + EXPECT_NO_THROW(cm_->write(time_, PERIOD)); EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_actuator->get_state().id()); @@ -555,7 +555,7 @@ TEST_P(TestControllerManagerWithTestableCM, stop_controllers_on_hardware_write_e EXPECT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_broadcaster_sensor->get_state().id()); - EXPECT_EQ(controller_interface::return_type::OK, cm_->update(TIME, PERIOD)); + EXPECT_EQ(controller_interface::return_type::OK, cm_->update(time_, PERIOD)); EXPECT_EQ(test_controller_actuator->internal_counter, previous_counter_lower) << "Execute has write error and it is not updated"; EXPECT_EQ(test_controller_system->internal_counter, previous_counter_higher) diff --git a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp index 16ba39d953..a34d70ada7 100644 --- a/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp +++ b/controller_manager/test/test_controllers_chaining_with_controller_manager.cpp @@ -378,7 +378,7 @@ class TestControllerChainingWithControllerManager position_tracking_controller->external_commands_for_testing_[0] = reference[0]; position_tracking_controller->external_commands_for_testing_[1] = reference[1]; - cm_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + cm_->update(time_, rclcpp::Duration::from_seconds(0.01)); cm_->resource_manager_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); // check if all controllers are updated diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index d67ae8a7d1..4137386f72 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -39,9 +39,9 @@ class TestLoadController : public ControllerManagerFixtureread(TIME, PERIOD); - cm_->update(TIME, PERIOD); - cm_->write(TIME, PERIOD); + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); }); update_executor_ =